diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3045b3746ea8e..12ad752b3ef29 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -79,7 +79,7 @@ evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.j launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp -launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +launch/tier4_map_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @@ -105,10 +105,10 @@ localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahir localization/yabloc/yabloc_monitor/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp -map/map_projection_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** azumi.suzuki@tier4.jp +map/map_height_fitter/** isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +map/map_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +map/map_projection_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +map/map_tf_generator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @@ -122,7 +122,7 @@ perception/euclidean_cluster/** yukihiro.saito@tier4.jp perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/image_projection_based_fusion/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @@ -156,13 +156,15 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_goal_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_planner_common/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@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/** 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 @@ -186,7 +188,7 @@ planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohs planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp @@ -226,13 +228,13 @@ system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabut system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp -system/system_diagnostic_graph/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index f9c29b81b6e6c..1da4d24966de9 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.11.0 + uses: styfle/cancel-workflow-action@0.12.0 with: workflow_id: all all_but_latest: true diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 95ebb8725f62b..b426d0cba6614 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -30,7 +30,7 @@ jobs: echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 ref: ${{ steps.set-tag-name.outputs.ref-name }} diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index e754ecab24f85..38738196a0bd3 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index b231dbda87938..33c00ee1066ae 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -16,7 +16,7 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ github.event.pull_request.head.ref }} diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index eb18ccdba38d0..1fbf2ff46925c 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Run spell-check uses: autowarefoundation/autoware-github-actions/spell-check@v1 diff --git a/common/autoware_auto_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md index f3a7eb5e58429..3d00f77c1b0c3 100644 --- a/common/autoware_auto_common/design/comparisons.md +++ b/common/autoware_auto_common/design/comparisons.md @@ -22,8 +22,8 @@ The `exclusive_or` function will test whether two values cast to different boole ## Example Usage ```c++ -#include "common/bool_comparisons.hpp" -#include "common/float_comparisons.hpp" +#include "autoware_auto_common/common/bool_comparisons.hpp" +#include "autoware_auto_common/common/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp similarity index 96% rename from common/autoware_auto_common/include/common/type_traits.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp index 7087ed1e81181..66f382f081b33 100644 --- a/common/autoware_auto_common/include/common/type_traits.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp @@ -14,15 +14,15 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" #include #include #include -#ifndef COMMON__TYPE_TRAITS_HPP_ -#define COMMON__TYPE_TRAITS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ namespace autoware { @@ -219,4 +219,4 @@ struct intersect } // namespace common } // namespace autoware -#endif // COMMON__TYPE_TRAITS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp similarity index 93% rename from common/autoware_auto_common/include/common/types.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/types.hpp index 3f3e444c1aa8c..1c7dfe48c7ec8 100644 --- a/common/autoware_auto_common/include/common/types.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp @@ -16,11 +16,11 @@ /// \file /// \brief This file includes common type definition -#ifndef COMMON__TYPES_HPP_ -#define COMMON__TYPES_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ -#include "common/visibility_control.hpp" -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include #include @@ -122,4 +122,4 @@ using void_t = void; } // namespace common } // namespace autoware -#endif // COMMON__TYPES_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp similarity index 88% rename from common/autoware_auto_common/include/common/visibility_control.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp index 0a988d6407dfa..33834fd9ccfed 100644 --- a/common/autoware_auto_common/include/common/visibility_control.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef COMMON__VISIBILITY_CONTROL_HPP_ -#define COMMON__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ #if defined(_MSC_VER) && defined(_WIN64) #if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) @@ -35,4 +35,4 @@ #error "Unsupported Build Configuration" #endif // _MSC_VER -#endif // COMMON__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp similarity index 89% rename from common/autoware_auto_common/include/helper_functions/angle_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp index 6cca2440d5680..ea974774dd9d5 100644 --- a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ -#define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ #include #include @@ -63,4 +63,4 @@ constexpr T wrap_angle(T angle) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp similarity index 85% rename from common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp index c6bf09365af4f..45da098ad0066 100644 --- a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp @@ -18,10 +18,10 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#include "common/types.hpp" +#include "autoware_auto_common/common/types.hpp" namespace autoware { @@ -47,4 +47,4 @@ types::bool8_t exclusive_or(const T & a, const T & b) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp similarity index 90% rename from common/autoware_auto_common/include/helper_functions/byte_reader.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp index d9e55c4ecfbfe..3852361caebeb 100644 --- a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__BYTE_READER_HPP_ -#define HELPER_FUNCTIONS__BYTE_READER_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ #include #include @@ -70,4 +70,4 @@ class ByteReader } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BYTE_READER_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp similarity index 88% rename from common/autoware_auto_common/include/helper_functions/crtp.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp index 0e8110a9a3bb9..e75674eb73c70 100644 --- a/common/autoware_auto_common/include/helper_functions/crtp.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__CRTP_HPP_ -#define HELPER_FUNCTIONS__CRTP_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ namespace autoware { @@ -49,4 +49,4 @@ class crtp } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__CRTP_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp similarity index 95% rename from common/autoware_auto_common/include/helper_functions/float_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp index de1f459f21d0a..1a64fe71a1720 100644 --- a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp @@ -18,8 +18,8 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ #include #include @@ -146,4 +146,4 @@ bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp similarity index 92% rename from common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp index fb9bdccf32b25..70c29eaf220d8 100644 --- a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ -#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ #include @@ -69,4 +69,4 @@ types::float32_t calculate_mahalanobis_distance( } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp similarity index 94% rename from common/autoware_auto_common/include/helper_functions/message_adapters.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index 352ef7c7b65d5..d3bda57b3374f 100644 --- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ -#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ #include @@ -112,4 +112,4 @@ TimeStamp get_stamp(const T & msg) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp similarity index 91% rename from common/autoware_auto_common/include/helper_functions/template_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp index b39931a3fd15a..0360908509618 100644 --- a/common/autoware_auto_common/include/helper_functions/template_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#include +#include "autoware_auto_common/common/types.hpp" #include @@ -72,4 +72,4 @@ struct expression_valid_with_return< } // namespace helper_functions } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp similarity index 84% rename from common/autoware_auto_common/include/helper_functions/type_name.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp index a95834373d552..106cede1f2f5a 100644 --- a/common/autoware_auto_common/include/helper_functions/type_name.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#define HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#include +#include "autoware_auto_common/common/visibility_control.hpp" #include #include @@ -53,4 +53,4 @@ COMMON_PUBLIC std::string get_type_name(const T &) } // namespace helper_functions } // namespace autoware -#endif // HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp index 031ae144139aa..810c302845daf 100644 --- a/common/autoware_auto_common/test/test_angle_utils.cpp +++ b/common/autoware_auto_common/test/test_angle_utils.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/angle_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp index 67c1c8ddbf9aa..a84d65e569692 100644 --- a/common/autoware_auto_common/test/test_bool_comparisons.cpp +++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/bool_comparisons.hpp" +#include "autoware_auto_common/helper_functions/bool_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp index c83d06c6e8132..a5ab8743f7ed4 100644 --- a/common/autoware_auto_common/test/test_byte_reader.cpp +++ b/common/autoware_auto_common/test/test_byte_reader.cpp @@ -14,9 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/types.hpp" -#include "gtest/gtest.h" -#include "helper_functions/byte_reader.hpp" +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/byte_reader.hpp" + +#include #include diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp index d292dc0a0cb20..37d3afdc177d5 100644 --- a/common/autoware_auto_common/test/test_float_comparisons.cpp +++ b/common/autoware_auto_common/test/test_float_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp index 262599180abb6..2015a85bc2bc8 100644 --- a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp +++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp @@ -13,8 +13,8 @@ // limitations under the License. // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/mahalanobis_distance.hpp" #include diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp index 34974c1cda9a6..c35f0ff826995 100644 --- a/common/autoware_auto_common/test/test_message_field_adapters.cpp +++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/message_adapters.hpp" #include diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index 9c9ca9ae4b5f2..a670aaab83cfa 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/template_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp index 1fb60d838f307..4ada59b4fb2e1 100644 --- a/common/autoware_auto_common/test/test_type_name.cpp +++ b/common/autoware_auto_common/test/test_type_name.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/type_name.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp index 7203ab8d649ee..92d01b3d84d51 100644 --- a/common/autoware_auto_common/test/test_type_traits.cpp +++ b/common/autoware_auto_common/test/test_type_traits.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/type_traits.hpp" +#include "autoware_auto_common/common/types.hpp" #include diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index 454e0e7ef044f..ee819b7cd797c 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -5,9 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED - include/geometry/spatial_hash.hpp - include/geometry/intersection.hpp - include/geometry/spatial_hash_config.hpp + include/autoware_auto_geometry/spatial_hash.hpp + include/autoware_auto_geometry/intersection.hpp + include/autoware_auto_geometry/spatial_hash_config.hpp src/spatial_hash.cpp src/bounding_box.cpp ) diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md index 26260ba8d8e67..4fe65ff8e0310 100644 --- a/common/autoware_auto_geometry/design/interval.md +++ b/common/autoware_auto_geometry/design/interval.md @@ -39,7 +39,7 @@ See 'Example Usage' below. ## Example Usage ```c++ -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" #include diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp similarity index 95% rename from common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp index d1dee63f73b56..0f3a68e14d442 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp @@ -17,11 +17,11 @@ /// \file /// \brief Common functionality for bounding box computation algorithms -#ifndef GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" #include #include @@ -185,4 +185,4 @@ std::vector GEOMETRY_PUBLIC get_transformed_corners } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp index f050628f32f25..e0f2e66e87ee5 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp @@ -19,10 +19,10 @@ /// bounding box // cspell: ignore eigenbox, EIGENBOX -#ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#include +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" #include #include @@ -244,4 +244,4 @@ BoundingBox eigenbox_2d(const IT begin, const IT end) } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp index 9b8991a7c7132..07fd6c989cedc 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp @@ -20,10 +20,10 @@ // cspell: ignore LFIT, lfit // LFIT means "L-Shape Fitting" -#ifndef GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#include +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" #include #include @@ -278,4 +278,4 @@ BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp index 5bc05810bb1b0..fb75384eb07cb 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp @@ -17,11 +17,11 @@ /// \file /// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes -#ifndef GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include -#include -#include +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include #include @@ -277,4 +277,4 @@ BoundingBox minimum_perimeter_bounding_box(std::list & list) } // namespace geometry } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp similarity index 71% rename from common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp index d1d84122889c9..c4c52928ac19a 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp @@ -15,12 +15,12 @@ // Co-developed by Tier IV, Inc. and Apex.AI, Inc. /// \file /// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define GEOMETRY__BOUNDING_BOX_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#include -#include -#include +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" +#include "autoware_auto_geometry/bounding_box/lfit.hpp" +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" namespace autoware { @@ -31,4 +31,4 @@ namespace geometry } // namespace geometry } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/common_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp index fd045003521ea..e3c2e58c9f80e 100644 --- a/common/autoware_auto_geometry/include/geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp @@ -16,12 +16,12 @@ /// \file /// \brief This file includes common functionality for 2D geometry, such as dot products -#ifndef GEOMETRY__COMMON_2D_HPP_ -#define GEOMETRY__COMMON_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" -#include +#include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -584,4 +584,4 @@ bool is_point_inside_polygon_2d( } // namespace common } // namespace autoware -#endif // GEOMETRY__COMMON_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp similarity index 93% rename from common/autoware_auto_geometry/include/geometry/common_3d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp index 4477b010e7eba..74cd210dec586 100644 --- a/common/autoware_auto_geometry/include/geometry/common_3d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp @@ -16,10 +16,10 @@ /// \file /// \brief This file includes common functionality for 3D geometry, such as dot products -#ifndef GEOMETRY__COMMON_3D_HPP_ -#define GEOMETRY__COMMON_3D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#include +#include "autoware_auto_geometry/common_2d.hpp" namespace autoware { @@ -74,4 +74,4 @@ inline OUT distance_3d(const T1 & a, const T2 & b) } // namespace common } // namespace autoware -#endif // GEOMETRY__COMMON_3D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/convex_hull.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp index e690c4d07441b..ae81c55ad6b55 100644 --- a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp @@ -18,11 +18,12 @@ /// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked /// lists of points -#ifndef GEOMETRY__CONVEX_HULL_HPP_ -#define GEOMETRY__CONVEX_HULL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" + +#include // lint -e537 NOLINT pclint vs cpplint #include @@ -191,4 +192,4 @@ typename std::list::const_iterator convex_hull(std::list & list) } // namespace common } // namespace autoware -#endif // GEOMETRY__CONVEX_HULL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp similarity index 94% rename from common/autoware_auto_geometry/include/geometry/hull_pockets.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp index 6e8caa0df1e80..cd9fd49f71635 100644 --- a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp @@ -18,11 +18,12 @@ /// \brief This file implements an algorithm for getting a list of "pockets" in the convex /// hull of a non-convex simple polygon. -#ifndef GEOMETRY__HULL_POCKETS_HPP_ -#define GEOMETRY__HULL_POCKETS_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" + +#include #include #include @@ -107,4 +108,4 @@ typename std::vector::value_typ } // namespace common } // namespace autoware -#endif // GEOMETRY__HULL_POCKETS_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/intersection.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp index 87dc32b0190d0..08c70c3a5a6be 100644 --- a/common/autoware_auto_geometry/include/geometry/intersection.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp @@ -14,11 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef GEOMETRY__INTERSECTION_HPP_ -#define GEOMETRY__INTERSECTION_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include #include @@ -309,4 +309,4 @@ common::types::float32_t convex_intersection_over_union_2d( } // namespace common } // namespace autoware -#endif // GEOMETRY__INTERSECTION_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/interval.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp index 59c26f27cc454..54be2c32b1d05 100644 --- a/common/autoware_auto_geometry/include/geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp @@ -18,11 +18,11 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef GEOMETRY__INTERVAL_HPP_ -#define GEOMETRY__INTERVAL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#include "common/types.hpp" -#include "helper_functions/float_comparisons.hpp" +#include +#include #include #include @@ -355,4 +355,4 @@ T Interval::clamp_to(const Interval & i, T val) } // namespace common } // namespace autoware -#endif // GEOMETRY__INTERVAL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/lookup_table.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp index e23a8c31b60f8..7b8867ca096ae 100644 --- a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp @@ -17,11 +17,12 @@ /// \file /// \brief This file contains a 1D linear lookup table implementation -#ifndef GEOMETRY__LOOKUP_TABLE_HPP_ -#define GEOMETRY__LOOKUP_TABLE_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#include "common/types.hpp" -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" + +#include #include #include @@ -175,4 +176,4 @@ class LookupTable1D } // namespace common } // namespace autoware -#endif // GEOMETRY__LOOKUP_TABLE_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/spatial_hash.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp index 78626548e5c74..8936e2c17d779 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp @@ -17,12 +17,13 @@ /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in /// 2D -#ifndef GEOMETRY__SPATIAL_HASH_HPP_ -#define GEOMETRY__SPATIAL_HASH_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#include -#include -#include +#include "autoware_auto_geometry/spatial_hash_config.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" + +#include #include #include @@ -328,4 +329,4 @@ using SpatialHash3d = SpatialHash; } // namespace common } // namespace autoware -#endif // GEOMETRY__SPATIAL_HASH_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp index e118ec24c7759..24c4d6e879d4a 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp @@ -17,14 +17,14 @@ /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in /// 2D -#ifndef GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#include "helper_functions/crtp.hpp" +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" -#include -#include -#include +#include +#include #include #include @@ -447,4 +447,4 @@ class GEOMETRY_PUBLIC Config3d : public Config } // namespace common } // namespace autoware -#endif // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp similarity index 90% rename from common/autoware_auto_geometry/include/geometry/visibility_control.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp index 96efe9aa6e27b..8972246997997 100644 --- a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define GEOMETRY__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -38,4 +38,4 @@ #else // defined(__linux__) #error "Unsupported Build Configuration" #endif // defined(__WIN32) -#endif // GEOMETRY__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp index 225ea099e4e79..3a4ea96a151a2 100644 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ b/common/autoware_auto_geometry/src/bounding_box.cpp @@ -14,13 +14,14 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" + #include -#include -#include // cspell: ignore eigenbox -#include +#include "autoware_auto_geometry/bounding_box/lfit.hpp" // cspell: ignore lfit -#include +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" #include diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp index 024cca48b8ee7..ffd91aaa08b3a 100644 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ b/common/autoware_auto_geometry/src/spatial_hash.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/spatial_hash.hpp" #include // lint -e537 NOLINT repeated include file due to cpplint rule diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp index 5e42622b19ce9..a179fbde5f151 100644 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -17,9 +17,9 @@ #ifndef TEST_BOUNDING_BOX_HPP_ #define TEST_BOUNDING_BOX_HPP_ -#include "geometry/bounding_box/lfit.hpp" +#include "autoware_auto_geometry/bounding_box/lfit.hpp" // cspell: ignore lfit -#include "geometry/bounding_box/rotating_calipers.hpp" +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" #include diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp index 50e946c416270..fc2d97c257b95 100644 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp @@ -17,7 +17,7 @@ #ifndef TEST_SPATIAL_HASH_HPP_ #define TEST_SPATIAL_HASH_HPP_ -#include "geometry/spatial_hash.hpp" +#include "autoware_auto_geometry/spatial_hash.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp index e7533518d7f49..81865656c55b5 100644 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -14,8 +14,9 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_geometry/lookup_table.hpp" + +#include #include diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp index d722314dada83..bc9c28682ed44 100644 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ b/common/autoware_auto_geometry/test/src/test_area.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/common_2d.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp index 642e396bdce31..baf6967edd47e 100644 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ b/common/autoware_auto_geometry/test/src/test_common_2d.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/common_2d.hpp" #include #include diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp index 43e3a3ad08adf..8b9bbce36c428 100644 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "geometry/convex_hull.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp index 2b79d4ff0f22b..9477a83a488ed 100644 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "geometry/convex_hull.hpp" -#include "geometry/hull_pockets.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" +#include "autoware_auto_geometry/hull_pockets.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp index 69c54960d4fc5..1036c69573c49 100644 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ b/common/autoware_auto_geometry/test/src/test_intersection.cpp @@ -14,8 +14,8 @@ // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_geometry/convex_hull.hpp" +#include "autoware_auto_geometry/intersection.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp index ba8d5742dadc5..266ab123f5203 100644 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ b/common/autoware_auto_geometry/test/src/test_interval.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 5d99b8a463711..deaa6be90b5f6 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -82,13 +83,15 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame @@ -98,6 +101,11 @@ get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( const std::string & uuid, const geometry_msgs::msg::Point & centroid, @@ -120,7 +128,7 @@ get_acceleration_text_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( @@ -133,14 +141,26 @@ get_path_confidence_marker_ptr( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 10bf11847c2f5..240cbdc9efc5b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,12 +14,12 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "rviz_common/properties/enum_property.hpp" +#include "common/color_alpha_property.hpp" -#include #include #include #include +#include #include #include #include @@ -77,6 +77,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_display_path_confidence_property{ "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", this}, + + m_display_existence_probability_property{ + "Display Existence Probability", false, "Enable/disable existence probability visualization", + this}, + m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, m_default_topic{default_topic} { @@ -91,6 +96,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); + // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -164,14 +170,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const ClassificationContainerT & labels, const double & line_width) const + const ClassificationContainerT & labels, const double & line_width, + const bool & is_orientation_available) const { const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); if (m_display_type_property->getOptionInt() == 0) { - return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); + return detail::get_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); } else if (m_display_type_property->getOptionInt() == 1) { return detail::get_2d_shape_marker_ptr( - shape_msg, centroid, orientation, color_rgba, line_width); + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); } else { return std::nullopt; } @@ -181,7 +189,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available); /// \brief Convert given shape msg into a Marker to visualize label name /// \tparam ClassificationContainerT List type with ObjectClassificationMsg @@ -201,6 +210,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return std::nullopt; } } + template + std::optional get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const ClassificationContainerT & labels) const + { + if (m_display_existence_probability_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_existence_probability_marker_ptr( + centroid, orientation, existence_probability, color_rgba); + } else { + return std::nullopt; + } + } template std::optional get_uuid_marker_ptr( @@ -254,10 +276,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const { if (m_display_twist_property.getBool()) { - return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance); + return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance, line_width); } else { return std::nullopt; } @@ -324,7 +347,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } return (it->second).label; } - std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const { std::stringstream ss; @@ -413,6 +435,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization rviz_common::properties::BoolProperty m_display_path_confidence_property; + + rviz_common::properties::BoolProperty m_display_existence_probability_property; + // Property to decide line width of object shape rviz_common::properties::FloatProperty m_line_width_property; // Default topic name to be visualized diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 9f00a2cb1cde2..a4cf8a703dff1 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -39,11 +39,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay Q_OBJECT public: + using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); +protected: + uint get_object_dynamics_to_visualize() + { + return m_select_object_dynamics_property->getOptionInt(); + } + + static bool is_object_to_show(const uint showing_dynamic_status, const TrackedObject & object); + private: + // Property to choose object dynamics to visualize + rviz_common::properties::EnumProperty * m_select_object_dynamics_property; + void processMessage(TrackedObjects::ConstSharedPtr msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp index ac9c5af4ddeef..57a42c95f9d34 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 067173288e685..db57e0e59a1ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -38,7 +38,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -57,6 +59,22 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = id++; + add_marker(existence_prob_marker_ptr); + } + // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5; @@ -73,7 +91,8 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 894e377a6f851..84321559f3541 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -56,7 +56,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_ptr->ns = std::string("path confidence"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->scale.x = 0.5; marker_ptr->scale.y = 0.5; marker_ptr->scale.z = 0.5; @@ -77,7 +77,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("path"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->pose = initPose(); marker_ptr->color = predicted_path_color; marker_ptr->color.a = 0.6; @@ -91,12 +91,12 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("twist"); - marker_ptr->scale.x = 0.03; + marker_ptr->scale.x = line_width; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; @@ -112,7 +112,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( pt_e.z = twist_with_covariance.twist.linear.z; marker_ptr->points.push_back(pt_e); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.0; @@ -137,7 +137,7 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( marker_ptr->text = std::to_string(static_cast(vel * 3.6)) + std::string("[km/h]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color = color_rgba; return marker_ptr; } @@ -158,7 +158,7 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color = color_rgba; return marker_ptr; } @@ -202,7 +202,7 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( point.y = e2.y() * sigma2; point.z = 0; marker_ptr->points.push_back(point); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; @@ -237,6 +237,23 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( marker_ptr->text = label; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color = color_rgba; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("existence probability"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->text = std::to_string(existence_probability); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); marker_ptr->color = color_rgba; return marker_ptr; @@ -245,7 +262,8 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -254,6 +272,9 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_bounding_box_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_cylinder_line_list(shape_msg, marker_ptr->points); @@ -267,7 +288,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; @@ -277,7 +298,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -286,6 +308,9 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); @@ -299,170 +324,127 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; return marker_ptr; } +void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points) +{ + geometry_msgs::msg::Point point; + for (int i = 0; i < num_pairs; ++i) { + point.x = point_list[point_pairs[i][0]][0]; + point.y = point_list[point_pairs[i][0]][1]; + point.z = point_list[point_pairs[i][0]][2]; + points.push_back(point); + point.x = point_list[point_pairs[i][1]][0]; + point.y = point_list[point_pairs[i][1]][1]; + point.z = point_list[point_pairs[i][1]][2]; + points.push_back(point); + } +} + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + geometry_msgs::msg::Point point; + + // bounding box corner points + // top and bottom surface, clockwise + const double point_list[8][3] = { + {length_half, width_half, height_half}, {length_half, -width_half, height_half}, + {-length_half, -width_half, height_half}, {-length_half, width_half, height_half}, + {length_half, width_half, -height_half}, {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, {-length_half, width_half, -height_half}, + }; + const int point_pairs[12][2] = { + {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7}, + }; + calc_line_list_from_points(point_list, point_pairs, 12, points); +} + +void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + // direction triangle + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double triangle_size_half = shape.dimensions.y / 1.4; geometry_msgs::msg::Point point; - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - // up surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, height_half}, + {length_half - triangle_size_half, width_half, height_half}, + {length_half - triangle_size_half, -width_half, height_half}, + {length_half, 0, -height_half}, + {length_half, width_half, height_half}, + {length_half, -width_half, height_half}, + }; + const int point_pairs[5][2] = { + {0, 1}, {1, 2}, {0, 2}, {3, 4}, {3, 5}, + }; + calc_line_list_from_points(point_list, point_pairs, 5, points); } void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + geometry_msgs::msg::Point point; + + // bounding box corner points + // top surface, clockwise + const double point_list[4][3] = { + {length_half, width_half, -height_half}, + {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, + {-length_half, width_half, -height_half}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {1, 2}, + {2, 3}, + {3, 0}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, points); +} + +void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double triangle_size_half = shape.dimensions.y / 1.4; geometry_msgs::msg::Point point; - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, -height_half}, + {length_half - triangle_size_half, width_half, -height_half}, + {length_half - triangle_size_half, -width_half, -height_half}, + }; + const int point_pairs[3][2] = { + {0, 1}, + {1, 2}, + {0, 2}, + }; + calc_line_list_from_points(point_list, point_pairs, 3, points); } void calc_cylinder_line_list( diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 2cc5397d18721..aa9eded82a88e 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -77,7 +77,7 @@ std::vector PredictedObjectsDisplay: auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), true); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -121,6 +121,25 @@ std::vector PredictedObjectsDisplay: markers.push_back(pose_with_covariance_marker_ptr); } + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = + object.kinematics.initial_pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = + object.kinematics.initial_pose_with_covariance.pose.position.y; + existence_probability_position.z = + object.kinematics.initial_pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, + object.kinematics.initial_pose_with_covariance.pose.orientation, existence_probability, + object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; @@ -154,7 +173,7 @@ std::vector PredictedObjectsDisplay: // Get marker for twist auto twist_marker = get_twist_marker_ptr( object.kinematics.initial_pose_with_covariance, - object.kinematics.initial_twist_with_covariance); + object.kinematics.initial_twist_with_covariance, get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 00a1199c697ce..fb53dbe1c2b8d 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -26,6 +26,24 @@ namespace object_detection { TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") { + // Option for selective visualization by object dynamics + m_select_object_dynamics_property = new rviz_common::properties::EnumProperty( + "Dynamic Status", "All", "Selectively visualize objects by its dynamic status.", this); + m_select_object_dynamics_property->addOption("Dynamic", 0); + m_select_object_dynamics_property->addOption("Static", 1); + m_select_object_dynamics_property->addOption("All", 2); +} + +bool TrackedObjectsDisplay::is_object_to_show( + const uint showing_dynamic_status, const TrackedObject & object) +{ + if (showing_dynamic_status == 0 && object.kinematics.is_stationary) { + return false; // Show only moving objects + } + if (showing_dynamic_status == 1 && !object.kinematics.is_stationary) { + return false; // Show only stationary objects + } + return true; } void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) @@ -33,12 +51,17 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) clear_markers(); update_id_map(msg); + const auto showing_dynamic_status = get_object_dynamics_to_visualize(); for (const auto & object : msg->objects) { + // Filter by object dynamic status + if (!is_object_to_show(showing_dynamic_status, object)) continue; + const auto line_width = get_line_width(); // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, - object.kinematics.pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + object.kinematics.pose_with_covariance.pose.orientation, object.classification, line_width, + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -81,7 +104,21 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(pose_with_covariance_marker_ptr); } - + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; @@ -113,7 +150,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, line_width); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index b35eb6e93ce6e..50c16ba8eaf7d 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -17,7 +17,7 @@ #ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ #define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#include +#include #include #include diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp index bc48e2a0294e0..97f46933f2fe6 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp @@ -52,7 +52,7 @@ struct NodeInterface {ServiceLog::CLIENT_RESPONSE, "client exit"}, {ServiceLog::ERROR_UNREADY, "client unready"}, {ServiceLog::ERROR_TIMEOUT, "client timeout"}}); - RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name); + RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name); ServiceLog msg; msg.stamp = node->now(); diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index d66d0fed3033c..e065f332b75e4 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,7 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include +#include #include #include diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index 3e982ac2ccf4d..532606248cd1f 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -34,6 +34,7 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { lanelet::projection::MGRSProjector projector{}; + projector.setMGRSCode(projector_info.mgrs_grid); return std::make_unique(projector); } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 516b3ab09e8b7..8c1d49fb5f607 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -39,6 +39,10 @@ std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio); } // namespace interpolation #endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index c3595d212f349..e44626498a80b 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -57,4 +57,15 @@ std::vector slerp( return query_values; } +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} } // namespace interpolation diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 43c8158b2f98b..f42deaa7f8c41 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -14,7 +14,7 @@ ament_auto_add_library(motion_utils SHARED src/trajectory/trajectory.cpp src/trajectory/interpolation.cpp src/trajectory/path_with_lane_id.cpp - src/trajectory/tmp_conversion.cpp + src/trajectory/conversion.cpp src/vehicle/vehicle_state_checker.cpp ) diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 87a0d54af6619..8bb5f13e3fb78 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#include "tier4_autoware_utils/system/backtrace.hpp" + #include #include #include @@ -25,6 +27,8 @@ namespace resample_utils { constexpr double close_s_threshold = 1e-6; +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; + template bool validate_size(const T & points) { @@ -58,23 +62,28 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { - std::cerr << "The number of resampling intervals is less than 2" << std::endl; + log_error( + "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - std::cerr << "resampling interval is longer than input points" << std::endl; + log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } @@ -86,20 +95,24 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - std::cerr << "resampling interval is less than " << motion_utils::overlap_threshold - << std::endl; + log_error( + "[resample_utils] invalid argument: resampling interval is less than " + + std::to_string(motion_utils::overlap_threshold)); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp new file mode 100644 index 0000000000000..7d4be216e89fe --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -0,0 +1,121 @@ +// Copyright 2021 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 MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ + +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" + +#include + +namespace motion_utils +{ +using TrajectoryPoints = std::vector; + +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + +template +autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::Path convertToPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_auto_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + +} // namespace motion_utils + +#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp deleted file mode 100644 index 28d783aa4d293..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2021 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 MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ - -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory); - -/** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f2e0c1c0c184c..b9136bc4002e3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -30,10 +30,12 @@ #include #include #include +#include #include #include namespace motion_utils { +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; /** * @brief validate if points container is empty or not @@ -44,7 +46,7 @@ void validateNonEmpty(const T & points) { if (points.empty()) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Points is empty."); + throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); } } @@ -82,7 +84,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."); + throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -214,7 +216,7 @@ std::optional searchZeroVelocityIndex( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -246,7 +248,7 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -336,7 +338,7 @@ std::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -401,12 +403,17 @@ double calcLongitudinalOffsetToSegment( const bool throw_exception = false) { if (seg_idx >= points.size() - 1) { - const auto error_message{"Segment index is invalid."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -418,18 +425,22 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return std::nan(""); } } if (seg_idx >= overlap_removed_points.size() - 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -575,18 +586,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -637,18 +654,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -684,7 +707,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -727,7 +750,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -779,7 +802,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -822,7 +845,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -861,7 +884,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -901,7 +924,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -929,7 +952,10 @@ calcArcLength>( template std::vector calcCurvature(const T & points) { - std::vector curvature_vec(points.size()); + std::vector curvature_vec(points.size(), 0.0); + if (points.size() < 3) { + return curvature_vec; + } for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); @@ -1007,7 +1033,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1041,17 +1067,22 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } if (points.size() - 1 < src_idx) { - const auto error_message{"Invalid source index"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return {}; } @@ -1118,7 +1149,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } @@ -1166,21 +1197,25 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } if (points.size() - 1 < src_idx) { - const auto error_message{"Invalid source index"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error(error_message); return {}; } if (points.size() == 1) { + log_error("Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1265,7 +1300,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1311,7 +1346,7 @@ std::optional insertTargetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1326,7 +1361,7 @@ std::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -2192,7 +2227,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate stop distance" + std::string(e.what())); return {}; } @@ -2331,9 +2366,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - RCLCPP_ERROR( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), - ". Return original points since cropped_points size is less than 2."); + log_error("Return original points since cropped_points size is less than 2."); return points; } @@ -2378,18 +2411,22 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } } if (overlap_removed_points.size() <= 1) { - const auto error_message{"points size is less than 2"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return 0 since no_throw option is enabled. The maintainer must check the code."); return 0.0; } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 1566b493c82f8..834d07a87ea09 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -486,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = resampled_path.right_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp similarity index 93% rename from common/motion_utils/src/trajectory/tmp_conversion.cpp rename to common/motion_utils/src/trajectory/conversion.cpp index 6f4a4039832bd..f198003d84091 100644 --- a/common/motion_utils/src/trajectory/tmp_conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include @@ -30,9 +30,11 @@ namespace motion_utils * points larger than the capacity. (Tier IV) */ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) + const std::vector & trajectory, + const std_msgs::msg::Header & header) { autoware_auto_planning_msgs::msg::Trajectory output{}; + output.header = header; for (const auto & pt : trajectory) output.points.push_back(pt); return output; } diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 80df1fabf562d..5794ed61e9e44 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 22567b569d0ad..eb6a06041e65d 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -1883,7 +1883,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1896,7 +1896,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1909,7 +1909,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2310,7 +2310,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4317,7 +4317,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4330,7 +4330,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4343,7 +4343,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } diff --git a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp index aedbe4b34ea22..16ff5a011536c 100644 --- a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp @@ -41,20 +41,20 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO(logger_, "client request"); + RCLCPP_DEBUG(logger_, "client request"); if (!client_->service_is_ready()) { - RCLCPP_INFO(logger_, "client available"); + RCLCPP_DEBUG(logger_, "client available"); return {response_error("Internal service is not available."), nullptr}; } auto future = client_->async_send_request(request); if (future.wait_for(timeout) != std::future_status::ready) { - RCLCPP_INFO(logger_, "client timeout"); + RCLCPP_DEBUG(logger_, "client timeout"); return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO(logger_, "client response"); + RCLCPP_DEBUG(logger_, "client response"); return {response_success(), future.get()}; } diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index 9610bf1e2b7f1..fdd270fcce2ef 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -263,9 +263,9 @@ void ManualController::onClickEnableButton() { auto req = std::make_shared(); req->engage = true; - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client_engage_->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client_engage_->async_send_request( 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 index 11dd050658aeb..a298967a31af9 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -38,9 +38,21 @@ Planning: - 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_path_planner_lane_change: + behavior_path_planner_goal_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner + + behavior_path_planner_start_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.start_planner + + behavior_path_avoidance_by_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE + + behavior_path_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.NORMAL behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index 5c17af5df22d1..69b61ba506b32 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -18,13 +18,13 @@ include_directories( ament_auto_add_library(tier4_planning_rviz_plugin SHARED # path point - include/path/display_base.hpp - include/path/display.hpp + include/tier4_planning_rviz_plugin/path/display_base.hpp + include/tier4_planning_rviz_plugin/path/display.hpp src/path/display.cpp # footprint - include/pose_with_uuid_stamped/display.hpp + include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp - include/mission_checkpoint/mission_checkpoint.hpp + include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp src/mission_checkpoint/mission_checkpoint.cpp src/tools/jsk_overlay_utils.cpp src/tools/max_velocity.cpp diff --git a/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp similarity index 93% rename from common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp index ea15788d789fe..964c418a3df1f 100644 --- a/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp @@ -45,8 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ -#define MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include @@ -88,4 +88,4 @@ private Q_SLOTS: } // namespace rviz_plugins -#endif // MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp similarity index 97% rename from common/tier4_planning_rviz_plugin/include/path/display.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 7d635f0cedcad..66b4a31f0993f 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH__DISPLAY_HPP_ -#define PATH__DISPLAY_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ -#include +#include "tier4_planning_rviz_plugin/path/display_base.hpp" #include #include @@ -228,4 +228,4 @@ class AutowareTrajectoryDisplay }; } // namespace rviz_plugins -#endif // PATH__DISPLAY_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp similarity index 99% rename from common/tier4_planning_rviz_plugin/include/path/display_base.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 74c2a60df3f32..4a59006e3bb96 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH__DISPLAY_BASE_HPP_ -#define PATH__DISPLAY_BASE_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #include #include @@ -40,9 +40,10 @@ #include #define EIGEN_MPL2_ONLY +#include "tier4_planning_rviz_plugin/utils.hpp" + #include #include -#include namespace { @@ -654,4 +655,4 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay }; } // namespace rviz_plugins -#endif // PATH__DISPLAY_BASE_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp similarity index 92% rename from common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp index f1fe3c30cd457..285a6902ccc1a 100644 --- a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ -#define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ #include @@ -81,4 +81,4 @@ private Q_SLOTS: } // namespace rviz_plugins -#endif // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/utils.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp similarity index 93% rename from common/tier4_planning_rviz_plugin/include/utils.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp index 76f9da0685908..94943a25f6a64 100644 --- a/common/tier4_planning_rviz_plugin/include/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS_HPP_ -#define UTILS_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ #include #include @@ -61,4 +61,4 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) } } // namespace rviz_plugins -#endif // UTILS_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ diff --git a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp index 6be2cfc426870..2ba5a309baa5c 100644 --- a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp +++ b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp @@ -45,7 +45,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 5bda4cdafd7e3..916201669e9e3 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "tier4_planning_rviz_plugin/path/display.hpp" + #include namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp index 8df04e04b2963..b525608a65625 100644 --- a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rviz_common/properties/tf_frame_property.hpp" +#include "tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp" -#include #include +#include #include #include #include diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index eba3e4eacd275..8f67a90215bd1 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -171,15 +171,15 @@ public Q_SLOTS: // NOLINT for Qt { auto req = std::make_shared(); - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_INFO( + RCLCPP_DEBUG( raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); }); diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index e27fc9b6cfa81..6ea142ed66a1b 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -174,9 +174,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::APPROACHING: label->setText("APPROACHING"); break; - case SteeringFactor::TRYING: - label->setText("TRYING"); - break; case SteeringFactor::TURNING: label->setText("TURNING"); break; diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 081eab28e8833..6ce95acc4ef1d 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,10 +15,13 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#include "autoware_perception_msgs/msg/traffic_signal.hpp" +#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" #include "tier4_perception_msgs/msg/traffic_signal.hpp" +#include #include #include #include @@ -36,6 +39,47 @@ bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal); void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence = -1); +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state); + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); tf2::Vector3 getTrafficLightBottomRight(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index de05355eafd66..37b4d46ce356a 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -6,6 +6,7 @@ The traffic_light_utils package Mingyu Li Shunsuke Miura + Satoshi Ota Apache License 2.0 ament_cmake_auto @@ -15,6 +16,7 @@ ament_lint_auto autoware_lint_common + autoware_perception_msgs lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 5a68223afb5ff..c8f2ca85b2089 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -46,11 +46,68 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; // the default value is -1, which means to not set confidence - if (confidence >= 0) { + if (confidence > 0) { signal.elements[0].confidence = confidence; } } +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light) { const auto & tl_bl = traffic_light.front(); diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index e88a5ee833612..960aadf226b70 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -358,25 +358,29 @@ bool AEB::hasCollision( const double current_v, const Path & ego_path, const std::vector & objects) { // calculate RSS - const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const auto current_p = tier4_autoware_utils::createPoint( + ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); const double & t = t_response_; for (const auto & obj : objects) { const double & obj_v = obj.velocity; const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - const double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - - vehicle_info_.max_longitudinal_offset_m; - if (dist_ego_to_object < rss_dist) { - // collision happens - ObjectData collision_data = obj; - collision_data.rss = rss_dist; - collision_data.distance_to_object = dist_ego_to_object; - collision_data_keeper_.update(collision_data); - return true; + + // check the object is front the ego or not + if ((obj.position.x - ego_path[0].position.x) > 0) { + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + ObjectData collision_data = obj; + collision_data.rss = rss_dist; + collision_data.distance_to_object = dist_ego_to_object; + collision_data_keeper_.update(collision_data); + return true; + } } } - return false; } diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp index 39ec316dbe437..8033ac9442960 100644 --- a/control/control_validator/include/control_validator/utils.hpp +++ b/control/control_validator/include/control_validator/utils.hpp @@ -15,7 +15,7 @@ #ifndef CONTROL_VALIDATOR__UTILS_HPP_ #define CONTROL_VALIDATOR__UTILS_HPP_ -#include +#include #include #include diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4518de37b7034..b8ee8ad79a33d 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -37,6 +37,7 @@ | `steering_angle_velocity` | double | steering angle velocity for operation | | `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | | `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | | `velocity_gain` | double | ratio to calculate velocity by acceleration | | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | @@ -85,3 +86,25 @@ | Autoware Disengage | â—‹ | | Vehicle Engage | â–³ | | Vehicle Disengage | â–³ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 8d48948a2d133..73a5d028985c5 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -9,6 +9,7 @@ accel_sensitivity: 1.0 brake_sensitivity: 1.0 control_command: + raw_control: false velocity_gain: 3.0 max_forward_velocity: 20.0 max_backward_velocity: 3.0 diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index c6de42afc38d9..22064f9cefaad 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node double brake_sensitivity_; // ControlCommand Parameter + bool raw_control_; double velocity_gain_; double max_forward_velocity_; double max_backward_velocity_; diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp new file mode 100644 index 0000000000000..a009ee1e12975 --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -0,0 +1,81 @@ +// 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. + +#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class XBOXJoyConverter : public JoyConverterBase +{ +public: + explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return LB(); } + bool turn_signal_right() const { return RB(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return ChangeView(); } + bool clear_emergency_stop() const { return Menu(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return LStickButton(); } + bool vehicle_disengage() const { return RStickButton(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(2); } + float RStickUpDown() const { return j_.axes.at(3); } + float RT() const { return j_.axes.at(4); } + float LT() const { return j_.axes.at(5); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(3); } + bool Y() const { return j_.buttons.at(4); } + bool LB() const { return j_.buttons.at(6); } + bool RB() const { return j_.buttons.at(7); } + bool Menu() const { return j_.buttons.at(11); } + bool LStickButton() const { return j_.buttons.at(13); } + bool RStickButton() const { return j_.buttons.at(14); } + bool ChangeView() const { return j_.buttons.at(15); } + bool Xbox() const { return j_.buttons.at(16); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index f719d8bd78cb5..d2804a7339046 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json index c67c575a6bd41..d4c6351324935 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -10,7 +10,7 @@ "type": "string", "description": "Joy controller type", "default": "DS4", - "enum": ["P65", "DS4", "G29"] + "enum": ["P65", "DS4", "G29", "XBOX"] }, "update_rate": { "type": "number", @@ -55,6 +55,11 @@ "control_command": { "type": "object", "properties": { + "raw_control": { + "type": "boolean", + "description": "Whether to skip input odometry", + "default": false + }, "velocity_gain": { "type": "number", "description": "Ratio to calculate velocity by acceleration", @@ -79,6 +84,7 @@ } }, "required": [ + "raw_control", "velocity_gain", "max_forward_velocity", "max_backward_velocity", diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 897986a7a41cf..5eec438032410 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -16,6 +16,7 @@ #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" #include "joy_controller/joy_converter/p65_joy_converter.hpp" +#include "joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt joy_ = std::make_shared(*msg); } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else if (joy_type_ == "XBOX") { + joy_ = std::make_shared(*msg); } else { joy_ = std::make_shared(*msg); } @@ -217,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady() } // Twist - { + if (!raw_control_) { if (!twist_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), @@ -458,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & steering_angle_velocity_ = declare_parameter("steering_angle_velocity"); accel_sensitivity_ = declare_parameter("accel_sensitivity"); brake_sensitivity_ = declare_parameter("brake_sensitivity"); + raw_control_ = declare_parameter("control_command.raw_control"); velocity_gain_ = declare_parameter("control_command.velocity_gain"); max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity"); max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity"); @@ -477,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); + if (!raw_control_) { + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + } else { + twist_ = std::make_shared(); + } // Publisher pub_control_command_ = diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/lane_departure_checker/schema/lane_departure_checker.json new file mode 100644 index 0000000000000..c7f39fbf7ef8a --- /dev/null +++ b/control/lane_departure_checker/schema/lane_departure_checker.json @@ -0,0 +1,79 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lane_departure_checker", + "type": "object", + "definitions": { + "lane_departure_checker": { + "type": "object", + "properties": { + "footprint_margin_scale": { + "type": "number", + "default": 1.0, + "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation." + }, + "resample_interval": { + "type": "number", + "default": 0.3, + "description": "Minimum Euclidean distance between points when resample trajectory.[m]." + }, + "max_deceleration": { + "type": "number", + "default": 2.8, + "description": "Maximum deceleration when calculating braking distance." + }, + "delay_time": { + "type": "number", + "default": 1.3, + "description": "Delay time which took to actuate brake when calculating braking distance. [second]." + }, + "max_lateral_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum lateral deviation in vehicle coordinate. [m]." + }, + "max_longitudinal_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum longitudinal deviation in vehicle coordinate. [m]." + }, + "max_yaw_deviation_deg": { + "type": "number", + "default": 60.0, + "description": "Maximum ego yaw deviation from trajectory. [deg]." + }, + "ego_nearest_dist_threshold": { + "type": "number" + }, + "ego_nearest_yaw_threshold": { + "type": "number" + }, + "min_braking_distance": { + "type": "number" + } + }, + "required": [ + "footprint_margin_scale", + "resample_interval", + "max_deceleration", + "max_lateral_deviation", + "max_longitudinal_deviation", + "max_yaw_deviation_deg", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "min_braking_distance" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lane_departure_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp index 07a6e2c6b0d21..8fbf3488c5a2e 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -120,10 +120,8 @@ bool filt_vector(const int num, std::vector & u) double tmp = 0.0; int num_tmp = 0; double count = 0; - if (i - num < 0) { - num_tmp = i; - } else if (i + num > static_cast(u.size()) - 1) { - num_tmp = static_cast(u.size()) - i - 1; + if ((i - num < 0) || (i + num > static_cast(u.size()) - 1)) { + num_tmp = std::min(i, static_cast(u.size()) - i - 1); } else { num_tmp = num; } diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp index 2cd641c031265..c68513586847b 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -61,6 +61,30 @@ TEST(TestLowpassFilter, MoveAverageFilter) EXPECT_EQ(filtered_vec[4], 23.0 / 3); EXPECT_EQ(filtered_vec[5], original_vec[5]); } + { + const int window_size = 3; + const std::vector original_vec = {1.0, 1.0, 1.0, 1.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 1.0); + EXPECT_EQ(filtered_vec[2], 1.0); + EXPECT_EQ(filtered_vec[3], original_vec[3]); + } + { + const int window_size = 4; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 8.0 / 3); + EXPECT_EQ(filtered_vec[2], 21.0 / 5); + EXPECT_EQ(filtered_vec[3], 30.0 / 5); + EXPECT_EQ(filtered_vec[4], 23.0 / 3); + EXPECT_EQ(filtered_vec[5], original_vec[5]); + } } TEST(TestLowpassFilter, Butterworth2dFilter) { diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index a110cb94d1d91..57eab2d87f18e 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -241,6 +241,10 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig ### STOPPED Parameter +The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. +Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. +If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. + | Name | Type | Description | Default value | | :----------- | :----- | :------------------------------------------- | :------------ | | stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..cc1c9c08887ba 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -16,6 +16,8 @@ #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" @@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel, const double current_acc); -/** - * @brief apply linear interpolation to orientation - * @param [in] o_from first orientation - * @param [in] o_to second orientation - * @param [in] ratio ratio between o_from and o_to for interpolation - */ -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point * @param [in] points trajectory points @@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); interpolated_point.pose.position.y = interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( + interpolated_point.pose.orientation = interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); interpolated_point.longitudinal_velocity_mps = interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..33a54663ccaef 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio) return v_from + (v_to - v_from) * ratio; } -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 12f0eece1e477..31c48fb5060a8 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -555,9 +555,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; }; - const auto info_throttle = [this](const auto & s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); - }; + const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); }; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; @@ -623,10 +621,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_control_state == ControlState::STOPPED) { // -- debug print -- if (has_nonzero_target_vel && !departure_condition_from_stopped) { - info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED."); + info_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } if (has_nonzero_target_vel && keep_stopped_condition) { - info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED."); + info_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); } // --------------- @@ -646,10 +644,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in EMERGENCY state if (m_control_state == ControlState::EMERGENCY) { + if (stopped_condition) { + return changeState(ControlState::STOPPED); + } + if (!emergency_condition) { - if (stopped_condition) { - return changeState(ControlState::STOPPED); - } if (!is_under_control) { // NOTE: On manual driving, no need stopping to exit the emergency. return changeState(ControlState::DRIVE); diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..58aa867b2deab 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -303,7 +305,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +313,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +322,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +330,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +338,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); 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 index d21b11e5b68de..1d7791955b576 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include #include -#include #include #include #include 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 index e3c3b5cccfb8f..3ce5728521141 100644 --- 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 @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index daaebaacde8de..ca0d77140b195 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -38,7 +38,7 @@ #include "trajectory_follower_base/lateral_controller_base.hpp" #include -#include +#include #include #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" diff --git a/evaluator/diagnostic_converter/include/converter_node.hpp b/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp similarity index 93% rename from evaluator/diagnostic_converter/include/converter_node.hpp rename to evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp index 53c762dac0ffe..59bb02ebf301f 100644 --- a/evaluator/diagnostic_converter/include/converter_node.hpp +++ b/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONVERTER_NODE_HPP_ -#define CONVERTER_NODE_HPP_ +#ifndef DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#define DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ #include @@ -63,4 +63,4 @@ class DiagnosticConverter : public rclcpp::Node }; } // namespace diagnostic_converter -#endif // CONVERTER_NODE_HPP_ +#endif // DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp index e61b19833d2fe..2a2574732694c 100644 --- a/evaluator/diagnostic_converter/src/converter_node.cpp +++ b/evaluator/diagnostic_converter/src/converter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "converter_node.hpp" +#include "diagnostic_converter/converter_node.hpp" #include diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp index 47f42d018ea71..167421f0777df 100644 --- a/evaluator/diagnostic_converter/test/test_converter_node.cpp +++ b/evaluator/diagnostic_converter/test/test_converter_node.cpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "converter_node.hpp" -#include "gtest/gtest.h" +#include "diagnostic_converter/converter_node.hpp" #include @@ -21,6 +20,8 @@ #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" +#include + #include #include #include diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 181f470a00800..ac7589ea2273b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -1,17 +1,13 @@ - - - - - - - - - - - - + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index e4b04d3d4a32e..bee300c587816 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -2,13 +2,16 @@ - - + + + + + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index d8f69c124526a..f75a181bfb659 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -7,6 +7,7 @@ Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda + Kento Yabuuchi Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 32a3fa102897d..fecdd29bcb7e9 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -474,7 +474,13 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, def launch_setup(context, *args, **kwargs): pipeline = GroundSegmentationPipeline(context) - components = [] + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + + components = [glog_component] components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 751eeea66c7b6..281a52a85af71 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -8,7 +8,7 @@ - + @@ -48,4 +48,18 @@ + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 7f5f9d61c4bf0..1b5753fb4d5c4 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -86,13 +86,18 @@ default="$(var data_path)/traffic_light_fine_detector" description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6" /> - + + @@ -231,6 +236,12 @@ + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 58e9e231e4aa0..7b81a42fdb79f 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -7,8 +7,10 @@ - - + + + + @@ -24,8 +26,10 @@ - - + + + + @@ -34,6 +38,8 @@ + + @@ -54,15 +60,18 @@ - - - + + + + + + @@ -72,11 +81,13 @@ - + + + @@ -84,6 +95,8 @@ + + @@ -104,15 +117,18 @@ - - - + + + + + + @@ -122,7 +138,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index a6bcb40e81252..dc5a2443d3f45 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -42,7 +42,15 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") add_launch_arg( - "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals" + "output/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ) + add_launch_arg( + "output/car/traffic_signals", "/perception/traffic_light_recognition/car/traffic_signals" + ) + add_launch_arg( + "output/pedestrian/traffic_signals", + "/perception/traffic_light_recognition/pedestrian/traffic_signals", ) # traffic_light_fine_detector @@ -63,15 +71,26 @@ def add_launch_arg(name: str, default_value=None, description=None): # traffic_light_classifier add_launch_arg("classifier_type", "1") add_launch_arg( - "classifier_model_path", + "car_classifier_model_path", os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), ) add_launch_arg( - "classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + "pedestrian_classifier_model_path", + os.path.join( + classifier_share_dir, "data", "pedestrian_traffic_light_classifier_efficientNet_b1.onnx" + ), + ) + add_launch_arg( + "car_classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + ) + add_launch_arg( + "pedestrian_classifier_label_path", + os.path.join(classifier_share_dir, "data", "lamp_labels_ped.txt"), ) add_launch_arg("classifier_precision", "fp16") add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") + add_launch_arg("backlight_threshold", "0.85") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -91,23 +110,56 @@ def create_parameter_dict(*args): ComposableNode( package="traffic_light_classifier", plugin="traffic_light::TrafficLightClassifierNodelet", - name="traffic_light_classifier", + name="car_traffic_light_classifier", + namespace="classification", + parameters=[ + { + "approximate_sync": LaunchConfiguration("approximate_sync"), + "classifier_type": LaunchConfiguration("classifier_type"), + "classify_traffic_light_type": 0, + "classifier_model_path": LaunchConfiguration("car_classifier_model_path"), + "classifier_label_path": LaunchConfiguration("car_classifier_label_path"), + "classifier_precision": LaunchConfiguration("classifier_precision"), + "classifier_mean": LaunchConfiguration("classifier_mean"), + "classifier_std": LaunchConfiguration("classifier_std"), + "backlight_threshold": LaunchConfiguration("backlight_threshold"), + } + ], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", LaunchConfiguration("output/rois")), + ("~/output/traffic_signals", "classified/car/traffic_signals"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="traffic_light_classifier", + plugin="traffic_light::TrafficLightClassifierNodelet", + name="pedestrian_traffic_light_classifier", namespace="classification", parameters=[ - create_parameter_dict( - "approximate_sync", - "classifier_type", - "classifier_model_path", - "classifier_label_path", - "classifier_precision", - "classifier_mean", - "classifier_std", - ) + { + "approximate_sync": LaunchConfiguration("approximate_sync"), + "classifier_type": LaunchConfiguration("classifier_type"), + "classify_traffic_light_type": 1, + "classifier_model_path": LaunchConfiguration( + "pedestrian_classifier_model_path" + ), + "classifier_label_path": LaunchConfiguration( + "pedestrian_classifier_label_path" + ), + "classifier_precision": LaunchConfiguration("classifier_precision"), + "classifier_mean": LaunchConfiguration("classifier_mean"), + "classifier_std": LaunchConfiguration("classifier_std"), + "backlight_threshold": LaunchConfiguration("backlight_threshold"), + } ], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), - ("~/output/traffic_signals", "classified/traffic_signals"), + ("~/output/traffic_signals", "classified/pedestrian/traffic_signals"), ], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} @@ -122,7 +174,10 @@ def create_parameter_dict(*args): ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), ("~/input/rough/rois", "detection/rough/rois"), - ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")), + ( + "~/input/traffic_signals", + LaunchConfiguration("output/traffic_signals"), + ), ("~/output/image", "debug/rois"), ("~/output/image/compressed", "debug/rois/compressed"), ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3a1f417106f54..1c7643359c496 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -16,17 +16,6 @@ - - - - - - - - - - - @@ -42,6 +31,9 @@ + + + @@ -95,7 +87,7 @@ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::AvoidanceByLaneChangeModuleManager, '")" if="$(var launch_avoidance_by_lane_change_module)" /> - + @@ -174,7 +166,12 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> - + + @@ -208,7 +205,6 @@ - @@ -254,6 +250,7 @@ + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 6492f20331a66..02f14a86cd7df 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -32,71 +32,33 @@ This package includes the following features:

-## Launch - -The `ekf_localizer` starts with the default parameters with the following command. - -```sh -roslaunch ekf_localizer ekf_localizer.launch -``` - -The parameters and input topic names can be set in the `ekf_localizer.launch` file. - ## Node ### Subscribed Topics -- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Input pose source with the measurement covariance matrix. - -- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - Input twist source with the measurement covariance matrix. - -- initialpose (geometry_msgs/PoseWithCovarianceStamped) - - Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. +| Name | Type | Description | +| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. | +| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. | +| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. | ### Published Topics -- ekf_odom (nav_msgs/Odometry) - - Estimated odometry. - -- ekf_pose (geometry_msgs/PoseStamped) - - Estimated pose. - -- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance. - -- ekf_biased_pose (geometry_msgs/PoseStamped) - - Estimated pose including the yaw bias - -- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance including the yaw bias - -- ekf_twist (geometry_msgs/TwistStamped) - - Estimated twist. - -- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - The estimated twist with covariance. - -- diagnostics (diagnostic_msgs/DiagnosticArray) - - The diagnostic information. +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | ### Published TF - base_link - - TF from "map" coordinate to estimated pose. + TF from `map` coordinate to estimated pose. ## Functions @@ -121,6 +83,7 @@ The parameters are set in `launch/ekf_localizer.launch` . | show_debug_info | bool | Flag to display debug info | false | | predict_frequency | double | Frequency for filtering and publishing [Hz] | 50.0 | | tf_rate | double | Frequency for tf broadcasting [Hz] | 10.0 | +| publish_tf | bool | Whether to publish tf | true | | extend_state_step | int | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50 | | enable_yaw_bias_estimation | bool | Flag to enable yaw bias estimation | true | @@ -152,6 +115,14 @@ The parameters are set in `launch/ekf_localizer.launch` . note: process noise for positions x & y are calculated automatically from nonlinear dynamics. +### Simple 1D Filter Parameters + +| Name | Type | Description | Default value | +| :-------------------- | :----- | :---------------------------------------------- | :------------ | +| z_filter_proc_dev | double | Simple1DFilter - Z filter process deviation | 1.0 | +| roll_filter_proc_dev | double | Simple1DFilter - Roll filter process deviation | 0.01 | +| pitch_filter_proc_dev | double | Simple1DFilter - Pitch filter process deviation | 0.01 | + ### For diagnostics | Name | Type | Description | Default value | @@ -197,7 +168,9 @@ Increasing the number will improve the smoothness of the estimation, but may hav -where `b_k` is the yawbias. +where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias. +$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link. +The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy. ### time delay model @@ -232,7 +205,7 @@ Note that, although the dimension gets larger since the analytical expansion can ## Known issues -- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. +- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. ## reference diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 1c16624605907..9de7f56f5c006 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -4,6 +4,7 @@ enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 + publish_tf: true extend_state_step: 50 # for Pose measurement @@ -22,6 +23,11 @@ proc_stddev_vx_c: 10.0 proc_stddev_wz_c: 5.0 + #Simple1DFilter parameters + z_filter_proc_dev: 1.0 + roll_filter_proc_dev: 0.01 + pitch_filter_proc_dev: 0.01 + # for diagnostics pose_no_update_count_threshold_warn: 50 pose_no_update_count_threshold_error: 100 diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 01ef658cf445d..5139f900a339e 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -28,6 +28,7 @@ class HyperParameters ekf_rate(node->declare_parameter("predict_frequency", 50.0)), ekf_dt(1.0 / std::max(ekf_rate, 0.1)), tf_rate_(node->declare_parameter("tf_rate", 10.0)), + publish_tf_(node->declare_parameter("publish_tf", true)), enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)), extend_state_step(node->declare_parameter("extend_state_step", 50)), pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))), @@ -40,6 +41,9 @@ class HyperParameters proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), + z_filter_proc_dev(node->declare_parameter("z_filter_proc_dev", 1.0)), + roll_filter_proc_dev(node->declare_parameter("roll_filter_proc_dev", 0.01)), + pitch_filter_proc_dev(node->declare_parameter("pitch_filter_proc_dev", 0.01)), pose_no_update_count_threshold_warn( node->declare_parameter("pose_no_update_count_threshold_warn", 50)), pose_no_update_count_threshold_error( @@ -57,6 +61,7 @@ class HyperParameters const double ekf_rate; const double ekf_dt; const double tf_rate_; + const bool publish_tf_; const bool enable_yaw_bias_estimation; const int extend_state_step; const std::string pose_frame_id; @@ -69,6 +74,9 @@ class HyperParameters const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise + const double z_filter_proc_dev; + const double roll_filter_proc_dev; + const double pitch_filter_proc_dev; const size_t pose_no_update_count_threshold_warn; const size_t pose_no_update_count_threshold_error; const size_t twist_no_update_count_threshold_warn; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index a3d2f52a4058c..b3ee3665cf45e 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -60,9 +60,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), std::bind(&EKFLocalizer::timerCallback, this)); - timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + if (params_.publish_tf_) { + timer_tf_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), + std::bind(&EKFLocalizer::timerTFCallback, this)); + } pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = @@ -94,9 +96,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - z_filter_.set_proc_dev(1.0); - roll_filter_.set_proc_dev(0.01); - pitch_filter_.set_proc_dev(0.01); + z_filter_.set_proc_dev(params_.z_filter_proc_dev); + roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); + pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev); } /* diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 573472fe2dabf..8977d82f34138 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -87,9 +87,15 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( { const double x = kalman_filter_.getXelement(IDX::X); const double y = kalman_filter_.getXelement(IDX::Y); + /* + getXelement(IDX::YAW) is surely `biased_yaw`. + Please note how `yaw` and `yaw_bias` are used in the state transition model and + how the observed pose is handled in the measurement pose update. + */ const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); const double yaw = biased_yaw + yaw_bias; + Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; @@ -224,7 +230,9 @@ bool EKFModule::measurementUpdatePose( return false; } - /* Set yaw */ + /* Since the kalman filter cannot handle the rotation angle directly, + offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less + than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index 86f3750ad11d9..49588a19ac620 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -43,68 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret. - -The four vertices of a landmark are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of a landmark, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### Example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -For example, when using the AR tag, it would look like this. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` +See ## About `consider_orientation` diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 6437455875cc2..272338905c3f0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -8,18 +8,24 @@ - + + + + - + + + + 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 7e83220dadf2a..857ec4f16f051 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -17,7 +17,6 @@ aruco cv_bridge diagnostic_msgs - image_transport landmark_manager lanelet2_extension localization_util diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index e569a71bbb5b0..43ac1e1098453 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -66,10 +66,6 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : Node("ar_tag_based_localizer", options), cam_info_received_(false) -{ -} - -bool ArTagBasedLocalizer::setup() { /* Declare node parameters @@ -92,7 +88,7 @@ bool ArTagBasedLocalizer::setup() } else { // Error RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); - return false; + return; } ekf_pose_buffer_ = std::make_unique( this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); @@ -111,59 +107,50 @@ bool ArTagBasedLocalizer::setup() tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); - /* - Initialize image transport - */ - it_ = std::make_unique(shared_from_this()); - /* Subscribers */ + using std::placeholders::_1; map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); + std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); - pose_array_pub_ = this->create_publisher("~/debug/detected_landmarks", qos_sub); image_sub_ = this->create_subscription( - "~/input/image", qos_sub, - std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); + "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, _1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", qos_sub, - std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, _1)); ekf_pose_sub_ = this->create_subscription( - "~/input/ekf_pose", qos_sub, - std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); + "~/input/ekf_pose", qos_sub, std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, _1)); /* Publishers */ - rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); - qos_marker.transient_local(); - qos_marker.reliable(); - marker_pub_ = this->create_publisher("~/debug/marker", qos_marker); - rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - image_pub_ = it_->advertise("~/debug/result", 1); - pose_pub_ = - this->create_publisher("~/output/pose_with_covariance", qos_pub); - diag_pub_ = this->create_publisher("/diagnostics", qos_pub); + const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); + const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + pose_pub_ = this->create_publisher( + "~/output/pose_with_covariance", qos_pub_periodic); + image_pub_ = this->create_publisher("~/debug/image", qos_pub_periodic); + mapped_tag_pose_pub_ = this->create_publisher("~/debug/mapped_tag", qos_pub_once); + detected_tag_pose_pub_ = + this->create_publisher("~/debug/detected_tag", qos_pub_periodic); + diag_pub_ = this->create_publisher("/diagnostics", qos_pub_periodic); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); - return true; } void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); - marker_pub_->publish(marker_msg); + mapped_tag_pose_pub_->publish(marker_msg); } void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { // check subscribers - if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { + if ((image_pub_->get_subscription_count() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); return; } @@ -186,22 +173,22 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; // detect - const std::vector landmarks = detect_landmarks(msg); + const std::vector landmarks = detect_landmarks(msg); if (landmarks.empty()) { return; } // for debug - if (pose_array_pub_->get_subscription_count() > 0) { + if (detected_tag_pose_pub_->get_subscription_count() > 0) { PoseArray pose_array_msg; pose_array_msg.header.stamp = sensor_stamp; pose_array_msg.header.frame_id = "map"; - for (const landmark_manager::Landmark & landmark : landmarks) { + for (const Landmark & landmark : landmarks) { const Pose detected_marker_on_map = tier4_autoware_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } - pose_array_pub_->publish(pose_array_msg); + detected_tag_pose_pub_->publish(pose_array_msg); } // calc new_self_pose @@ -267,24 +254,12 @@ void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & m return; } - cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - camera_matrix.at(0, 0) = msg->p[0]; - camera_matrix.at(0, 1) = msg->p[1]; - camera_matrix.at(0, 2) = msg->p[2]; - camera_matrix.at(0, 3) = msg->p[3]; - camera_matrix.at(1, 0) = msg->p[4]; - camera_matrix.at(1, 1) = msg->p[5]; - camera_matrix.at(1, 2) = msg->p[6]; - camera_matrix.at(1, 3) = msg->p[7]; - camera_matrix.at(2, 0) = msg->p[8]; - camera_matrix.at(2, 1) = msg->p[9]; - camera_matrix.at(2, 2) = msg->p[10]; - camera_matrix.at(2, 3) = msg->p[11]; - - cv::Mat distortion_coeff(4, 1, CV_64FC1); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } + // copy camera matrix + cv::Mat camera_matrix(3, 4, CV_64FC1); + std::copy(msg->p.begin(), msg->p.end(), camera_matrix.begin()); + + // all 0 + cv::Mat distortion_coeff(4, 1, CV_64FC1, 0.0); const cv::Size size(static_cast(msg->width), static_cast(msg->height)); @@ -318,7 +293,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( cv_ptr->image.copyTo(in_image); } catch (cv_bridge::Exception & e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return std::vector{}; + return std::vector{}; } // get transform from base_link to camera @@ -328,7 +303,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); - return std::vector{}; + return std::vector{}; } // detect @@ -336,7 +311,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( detector_.detect(in_image, markers, cam_param_, marker_size_, false); // parse - std::vector landmarks; + std::vector landmarks; for (aruco::Marker & marker : markers) { // convert marker pose to tf @@ -352,7 +327,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z); if (distance <= distance_threshold_) { tf2::doTransform(pose, pose, transform_sensor_to_base_link); - landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose}); + landmarks.push_back(Landmark{std::to_string(marker.id), pose}); } // for debug, drawing the detected markers @@ -361,12 +336,12 @@ std::vector ArTagBasedLocalizer::detect_landmarks( } // for debug - if (image_pub_.getNumSubscribers() > 0) { + if (image_pub_->get_subscription_count() > 0) { cv_bridge::CvImage out_msg; out_msg.header.stamp = sensor_stamp; out_msg.encoding = sensor_msgs::image_encodings::RGB8; out_msg.image = in_image; - image_pub_.publish(out_msg.toImageMsg()); + image_pub_->publish(*out_msg.toImageMsg()); } return landmarks; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index b7dfb45a26ce3..f70821a39ffe8 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -48,11 +48,12 @@ #include "landmark_manager/landmark_manager.hpp" #include "localization_util/smart_pose_buffer.hpp" -#include #include #include #include +#include +#include #include #include @@ -79,17 +80,17 @@ class ArTagBasedLocalizer : public rclcpp::Node using TransformStamped = geometry_msgs::msg::TransformStamped; using MarkerArray = visualization_msgs::msg::MarkerArray; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using Landmark = landmark_manager::Landmark; public: explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - bool setup(); private: void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); - std::vector detect_landmarks(const Image::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); // Parameters float marker_size_{}; @@ -104,9 +105,6 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - // image transport - std::unique_ptr it_; - // Subscribers rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; @@ -114,10 +112,10 @@ class ArTagBasedLocalizer : public rclcpp::Node rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers - rclcpp::Publisher::SharedPtr marker_pub_; - rclcpp::Publisher::SharedPtr pose_array_pub_; - image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr detected_tag_pose_pub_; + rclcpp::Publisher::SharedPtr mapped_tag_pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; // Others diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp index cbcd57b4b222a..8ef1dd6195580 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp @@ -48,7 +48,6 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); std::shared_ptr ptr = std::make_shared(); - ptr->setup(); rclcpp::spin(ptr); rclcpp::shutdown(); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 1b44327fd9e8b..5d05dd7e3755a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -55,7 +55,8 @@ class TestArTagBasedLocalizer : public ::testing::Test TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT { - EXPECT_TRUE(node_->setup()); + // Check if the constructor finishes successfully + EXPECT_TRUE(true); } int main(int argc, char ** argv) diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index fba419f746b5e..c2b0751d91f9a 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -15,6 +15,8 @@ #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#include "lanelet2_extension/localization/landmark.hpp" + #include #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" @@ -40,13 +42,15 @@ class LandmarkManager public: void parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger); + const std::string & target_subtype); + + [[nodiscard]] std::vector get_landmarks() const; [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const; [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose( - const std::vector & detected_landmarks, - const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const; + const std::vector & detected_landmarks, const geometry_msgs::msg::Pose & self_pose, + const bool consider_orientation) const; private: // To allow multiple landmarks with the same id to be registered on a vector_map, diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 875f04edd8c47..57bfcde461af6 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -29,32 +29,17 @@ namespace landmark_manager void LandmarkManager::parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger) + const std::string & target_subtype) { - RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version); - RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format); - RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version); - RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size()); - lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - - for (const auto & poly : lanelet_map_ptr->polygonLayer) { - const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; - if (type != "pose_marker") { - continue; - } - const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")}; - if (subtype != target_subtype) { - continue; - } - + std::vector landmarks = + lanelet::localization::parseLandmarkPolygons(msg, target_subtype); + for (const lanelet::Polygon3d & poly : landmarks) { // Get landmark_id const std::string landmark_id = poly.attributeOr("marker_id", "none"); // Get 4 vertices const auto & vertices = poly.basicPolygon(); if (vertices.size() != 4) { - RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4"); continue; } @@ -65,12 +50,8 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - RCLCPP_INFO_STREAM(logger, "volume = " << volume); const double volume_threshold = 1e-5; if (volume > volume_threshold) { - RCLCPP_WARN_STREAM( - logger, - "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane."); continue; } @@ -99,16 +80,25 @@ void LandmarkManager::parse_landmarks( // Add landmarks_map_[landmark_id].push_back(pose); - RCLCPP_INFO_STREAM(logger, "id: " << landmark_id); - RCLCPP_INFO_STREAM( - logger, - "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); - RCLCPP_INFO_STREAM( - logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " - << pose.orientation.z << ", " << pose.orientation.w); } } +std::vector LandmarkManager::get_landmarks() const +{ + std::vector landmarks; + + landmark_manager::Landmark landmark; + for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) { + for (const auto & pose : landmark_poses) { + landmark.id = landmark_id_str; + landmark.pose = pose; + landmarks.push_back(landmark); + } + } + + return landmarks; +} + visualization_msgs::msg::MarkerArray LandmarkManager::get_landmarks_as_marker_array_msg() const { int32_t id = 0; diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index ade446020d101..fe65077d89649 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp - src/tf2_listener_module.cpp src/smart_pose_buffer.cpp ) diff --git a/localization/localization_util/include/localization_util/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp deleted file mode 100644 index b332f9effe0e0..0000000000000 --- a/localization/localization_util/include/localization_util/tf2_listener_module.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ -#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ - -#include - -#include -#include -#include - -#include - -class Tf2ListenerModule -{ - using TransformStamped = geometry_msgs::msg::TransformStamped; - -public: - explicit Tf2ListenerModule(rclcpp::Node * node); - bool get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) const; - -private: - rclcpp::Logger logger_; - tf2_ros::Buffer tf2_buffer_; - tf2_ros::TransformListener tf2_listener_; -}; - -#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ diff --git a/localization/localization_util/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp deleted file mode 100644 index 8a19ceec9f30d..0000000000000 --- a/localization/localization_util/src/tf2_listener_module.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_util/tf2_listener_module.hpp" - -#include - -geometry_msgs::msg::TransformStamped identity_transform_stamped( - const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, - const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = timestamp; - transform.header.frame_id = header_frame_id; - transform.child_frame_id = child_frame_id; - transform.transform.rotation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); - transform.transform.translation = tier4_autoware_utils::createTranslation(0.0, 0.0, 0.0); - return transform; -} - -Tf2ListenerModule::Tf2ListenerModule(rclcpp::Node * node) -: logger_(node->get_logger()), tf2_buffer_(node->get_clock()), tf2_listener_(tf2_buffer_) -{ -} - -bool Tf2ListenerModule::get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, const TransformStamped::SharedPtr & transform_stamped_ptr) const -{ - const TransformStamped identity = - identity_transform_stamped(timestamp, target_frame, source_frame); - - if (target_frame == source_frame) { - *transform_stamped_ptr = identity; - return true; - } - - try { - *transform_stamped_ptr = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(logger_, "%s", ex.what()); - RCLCPP_ERROR(logger_, "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - *transform_stamped_ptr = identity; - return false; - } - return true; -} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 2ad4a47b833d1..b4d656cb1810b 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -30,7 +30,6 @@ ament_auto_add_executable(ndt_scan_matcher src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/map_module.cpp src/map_update_module.cpp ) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d7a7b1c5c37f3..95a7cebdc01c8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | ----------------------------------- | ----------------------------------------------- | ------------------------------------- | | `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | -| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | | `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped` | base position for regularization term | @@ -193,12 +192,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Additional interfaces -#### Additional inputs - -| Name | Type | Description | -| ---------------- | ------------------------- | ----------------------------------------------------------- | -| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | - #### Additional outputs | Name | Type | Description | @@ -213,20 +206,15 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | -------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | - -### Enabling the dynamic map loading feature +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------ | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | -To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. -Follow the next two instructions. +### Notes for dynamic map loading -1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -2. split the PCD files into grids (recommended size: 20[m] x 20[m]) +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m]) Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of @@ -235,14 +223,10 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -| :------------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| multiple files | true | true | dynamically | -| multiple files | true | false | **does NOT work** | -| multiple files | false | true/false | at once (standard) | +| PCD files | How NDT loads map(s) | +| :------------: | :------------------: | +| single file | at once (standard) | +| multiple files | dynamically | ## Scan matching score based on de-grounded LiDAR scan 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 1a6c26cd9c351..9bc62d3f919c6 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - # Vehicle reference frame base_frame: "base_link" diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp deleted file mode 100644 index 79454e89b9ed0..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 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 NDT_SCAN_MATCHER__MAP_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ - -#include - -#include - -#include -#include -#include - -#include - -class MapModule -{ - using PointSource = pcl::PointXYZ; - using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - -public: - MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group); - -private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); - - rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; -}; - -#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index da0ae27a7808e..8b192b0186b42 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,7 +15,6 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" @@ -48,14 +47,10 @@ class MapUpdateModule public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group); + std::shared_ptr ndt_ptr); private: friend class NDTScanMatcher; - void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); - void map_update_timer_callback(); void update_ndt( const std::vector & maps_to_add, @@ -68,21 +63,13 @@ class MapUpdateModule rclcpp::Client::SharedPtr pcd_loader_client_; - rclcpp::TimerBase::SharedPtr map_update_timer_; - - rclcpp::Subscription::SharedPtr ekf_odom_sub_; - - rclcpp::CallbackGroup::SharedPtr map_callback_group_; std::shared_ptr ndt_ptr_; std::mutex * ndt_ptr_mutex_; - std::string map_frame_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - std::shared_ptr tf2_listener_module_; std::optional last_update_position_ = std::nullopt; - std::optional current_position_ = std::nullopt; const double dynamic_map_loading_update_distance_; const double dynamic_map_loading_map_radius_; const double lidar_radius_; 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 7f4a17a65a7e1..e9aa265c78f34 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 @@ -18,8 +18,6 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" -#include "localization_util/tf2_listener_module.hpp" -#include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -41,6 +39,8 @@ #include #include #include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -88,6 +88,7 @@ class NDTScanMatcher : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); + void callback_timer(); void callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); void callback_initial_pose( @@ -124,6 +125,7 @@ class NDTScanMatcher : public rclcpp::Node const double score, const double score_threshold, const std::string & score_name); bool validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood); + static int count_oscillation(const std::vector & result_pose_msg_array); std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, @@ -133,6 +135,7 @@ class NDTScanMatcher : public rclcpp::Node void publish_diagnostic(); + rclcpp::TimerBase::SharedPtr map_update_timer_; rclcpp::Subscription::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; rclcpp::Subscription::SharedPtr @@ -173,6 +176,10 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_trigger_node_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; std::shared_ptr ndt_ptr_; std::shared_ptr> state_ptr_; @@ -191,8 +198,6 @@ class NDTScanMatcher : public rclcpp::Node double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; - float inversion_vector_threshold_; - float oscillation_threshold_; bool use_cov_estimation_; std::vector initial_pose_offset_model_; std::array output_pose_covariance_; @@ -200,13 +205,16 @@ class NDTScanMatcher : public rclcpp::Node std::mutex ndt_ptr_mtx_; std::unique_ptr initial_pose_buffer_; + // Keep latest position for dynamic map loading + // This variable is only used when use_dynamic_map_loading is true + std::mutex latest_ekf_position_mtx_; + std::optional latest_ekf_position_ = std::nullopt; + // variables for regularization const bool regularization_enabled_; // whether to use longitudinal regularization std::unique_ptr regularization_pose_buffer_; - bool is_activated_; - std::shared_ptr tf2_listener_module_; - std::unique_ptr map_module_; + std::atomic is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; @@ -214,8 +222,6 @@ class NDTScanMatcher : public rclcpp::Node bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; - bool use_dynamic_map_loading_; - // The execution time which means probably NDT cannot matches scans properly int64_t critical_upper_bound_exe_time_ms_; }; diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index b442ac1b3d843..cbcb0a9f74bc4 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -2,32 +2,28 @@ - - - - + + + - - + - - + - + + - - + - diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp deleted file mode 100644 index d6088a1b14949..0000000000000 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 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 "ndt_scan_matcher/map_module.hpp" - -MapModule::MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) -{ - auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; - - map_points_sub_ = node->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt); -} - -void MapModule::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - NormalDistributionsTransform new_ndt; - new_ndt.setParams(ndt_ptr_->getParams()); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt.setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt.align(*output_cloud); - - // swap - ndt_ptr_mutex_->lock(); - *ndt_ptr_ = new_ndt; - ndt_ptr_mutex_->unlock(); -} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index f0a583417fb76..39b92fe248660 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -14,90 +14,37 @@ #include "ndt_scan_matcher/map_update_module.hpp" -template -double norm_xy(const T p1, const U p2) -{ - double dx = p1.x - p2.x; - double dy = p1.y - p2.y; - return std::sqrt(dx * dx + dy * dy); -} - MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group) + std::shared_ptr ndt_ptr) : ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), - map_frame_(std::move(map_frame)), logger_(node->get_logger()), clock_(node->get_clock()), - tf2_listener_module_(std::move(tf2_listener_module)), dynamic_map_loading_update_distance_( node->declare_parameter("dynamic_map_loading_update_distance")), dynamic_map_loading_map_radius_( node->declare_parameter("dynamic_map_loading_map_radius")), lidar_radius_(node->declare_parameter("lidar_radius")) { - auto main_sub_opt = rclcpp::SubscriptionOptions(); - main_sub_opt.callback_group = main_callback_group; - - map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - ekf_odom_sub_ = node->create_subscription( - "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), - main_sub_opt); - loaded_pcd_pub_ = node->create_publisher( "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); pcd_loader_client_ = node->create_client("pcd_loader_service"); - - double map_update_dt = 1.0; - auto period_ns = std::chrono::duration_cast( - std::chrono::duration(map_update_dt)); - map_update_timer_ = rclcpp::create_timer( - node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), - map_callback_group_); } -void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const { - current_position_ = odom_ptr->pose.pose.position; - if (last_update_position_ == std::nullopt) { - return; + return false; } - double distance = norm_xy(current_position_.value(), last_update_position_.value()); + const double dx = position.x - last_update_position_.value().x; + const double dy = position.y - last_update_position_.value().y; + const double distance = std::hypot(dx, dy); if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); - } -} - -void MapUpdateModule::map_update_timer_callback() -{ - if (current_position_ == std::nullopt) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 1, - "Cannot find the reference position for map update. Please check if the EKF odometry is " - "provided to NDT."); - return; + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); } - if (last_update_position_ == std::nullopt) return; - - // continue only if we should update the map - if (should_update_map(current_position_.value())) { - RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); - update_map(current_position_.value()); - last_update_position_ = current_position_; - } -} - -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const -{ - if (last_update_position_ == std::nullopt) return false; - double distance = norm_xy(position, last_update_position_.value()); return distance > dynamic_map_loading_update_distance_; } @@ -110,10 +57,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); } // send a request to map_loader @@ -145,21 +89,30 @@ void MapUpdateModule::update_ndt( } const auto exe_start_time = std::chrono::system_clock::now(); - NormalDistributionsTransform backup_ndt = *ndt_ptr_; + const size_t add_size = maps_to_add.size(); + + // Perform heavy processing outside of the lock scope + std::vector>> points_pcl(add_size); + for (size_t i = 0; i < add_size; i++) { + points_pcl[i] = pcl::make_shared>(); + pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); + } + + (*ndt_ptr_mutex_).lock(); // Add pcd - for (const auto & map_to_add : maps_to_add) { - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); - backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + for (size_t i = 0; i < add_size; i++) { + ndt_ptr_->addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - backup_ndt.removeTarget(map_id_to_remove); + ndt_ptr_->removeTarget(map_id_to_remove); } - backup_ndt.createVoxelKdtree(); + ndt_ptr_->createVoxelKdtree(); + + (*ndt_ptr_mutex_).unlock(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = @@ -167,14 +120,6 @@ void MapUpdateModule::update_ndt( const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); - // swap - (*ndt_ptr_mutex_).lock(); - // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be avoided. But I will leave this for now since I cannot come up with a solution other - // than using pointer of pointer. - *ndt_ptr_ = backup_ndt; - (*ndt_ptr_mutex_).unlock(); - publish_partial_pcd_map(); } 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 783df34e6c0f8..0f6fdbc15db26 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -64,47 +64,19 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -bool validate_local_optimal_solution_oscillation( - const std::vector & result_pose_msg_array, - const float oscillation_threshold, const float inversion_vector_threshold) -{ - bool prev_oscillation = false; - int oscillation_cnt = 0; - - for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { - const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); - const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); - const Eigen::Vector3d prev_prev_pose = - point_to_vector3d(result_pose_msg_array.at(i - 2).position); - const auto current_vec = current_pose - prev_pose; - const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); - const bool oscillation = prev_vec.dot(current_vec) < inversion_vector_threshold; - if (prev_oscillation && oscillation) { - if (static_cast(oscillation_cnt) > oscillation_threshold) { - return true; - } - ++oscillation_cnt; - } else { - oscillation_cnt = 0; - } - prev_oscillation = oscillation; - } - return false; -} - // cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - 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 output_pose_covariance_({}), - regularization_enabled_(declare_parameter("regularization_enabled")) + regularization_enabled_(declare_parameter("regularization_enabled")), + is_activated_(false) { (*state_ptr_)["state"] = "Initializing"; - is_activated_ = false; int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); points_queue_size = std::max(points_queue_size, (int64_t)0); @@ -193,31 +165,36 @@ NDTScanMatcher::NDTScanMatcher() z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); - rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; - initial_pose_callback_group = + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroup::SharedPtr sensor_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - rclcpp::CallbackGroup::SharedPtr main_callback_group; - main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto initial_pose_sub_opt = rclcpp::SubscriptionOptions(); initial_pose_sub_opt.callback_group = initial_pose_callback_group; - - auto main_sub_opt = rclcpp::SubscriptionOptions(); - main_sub_opt.callback_group = main_callback_group; - + auto sensor_sub_opt = rclcpp::SubscriptionOptions(); + sensor_sub_opt.callback_group = sensor_callback_group; + + constexpr double map_update_dt = 1.0; + constexpr auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + this, this->get_clock(), period_ns, std::bind(&NDTScanMatcher::callback_timer, this), + timer_callback_group_); initial_pose_sub_ = this->create_subscription( "ekf_pose_with_covariance", 100, std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); sensor_points_sub_ = this->create_subscription( "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), - std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); + std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), + sensor_sub_opt); // Only if regularization is enabled, subscribe to the regularization base pose if (regularization_enabled_) { // NOTE: The reason that the regularization subscriber does not belong to the - // main_callback_group is to ensure that the regularization callback is called even if + // sensor_callback_group is to ensure that the regularization callback is called even if // sensor_callback takes long time to process. // Both callback_initial_pose and callback_regularization_pose must not miss receiving data for // proper interpolation. @@ -280,22 +257,14 @@ NDTScanMatcher::NDTScanMatcher() "ndt_align_srv", std::bind( &NDTScanMatcher::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); service_trigger_node_ = this->create_service( "trigger_node_srv", std::bind( &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); - - tf2_listener_module_ = std::make_shared(this); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group); - } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - } + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); logger_configure_ = std::make_unique(this); } @@ -368,6 +337,26 @@ void NDTScanMatcher::publish_diagnostic() diagnostics_pub_->publish(diag_msg); } +void NDTScanMatcher::callback_timer() +{ + if (!is_activated_) { + return; + } + std::lock_guard lock(latest_ekf_position_mtx_); + if (latest_ekf_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); + return; + } + // continue only if we should update the map + if (map_update_module_->should_update_map(latest_ekf_position_.value())) { + RCLCPP_INFO(this->get_logger(), "Start updating NDT map (timer_callback)"); + map_update_module_->update_map(latest_ekf_position_.value()); + } +} + void NDTScanMatcher::callback_initial_pose( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { @@ -382,6 +371,12 @@ void NDTScanMatcher::callback_initial_pose( << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ << ". Please check the frame_id in the input topic and ensure it is correct."); } + + { + // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock + std::lock_guard lock(latest_ekf_position_mtx_); + latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; + } } void NDTScanMatcher::callback_regularization_pose( @@ -473,10 +468,11 @@ void NDTScanMatcher::callback_sensor_points( // perform several validations bool is_ok_iteration_num = validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); + const int oscillation_num = count_oscillation(transformation_msg_array); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { - is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( - transformation_msg_array, oscillation_threshold_, inversion_vector_threshold_); + constexpr int oscillation_threshold = 10; + is_local_optimal_solution_oscillation = (oscillation_num > oscillation_threshold); } bool is_ok_converged_param = validate_converged_param( ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); @@ -558,11 +554,9 @@ void NDTScanMatcher::callback_sensor_points( std::to_string(ndt_result.nearest_voxel_transformation_likelihood); (*state_ptr_)["iteration_num"] = std::to_string(ndt_result.iteration_num); (*state_ptr_)["skipping_publish_num"] = std::to_string(skipping_publish_num); - if (is_local_optimal_solution_oscillation) { - (*state_ptr_)["is_local_optimal_solution_oscillation"] = "1"; - } else { - (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; - } + (*state_ptr_)["oscillation_count"] = std::to_string(oscillation_num); + (*state_ptr_)["is_local_optimal_solution_oscillation"] = + std::to_string(is_local_optimal_solution_oscillation); (*state_ptr_)["execution_time"] = std::to_string(exe_time); publish_diagnostic(); @@ -573,11 +567,25 @@ void NDTScanMatcher::transform_sensor_measurement( const pcl::shared_ptr> & sensor_points_input_ptr, pcl::shared_ptr> & sensor_points_output_ptr) { - auto tf_target_to_source_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), target_frame, source_frame, tf_target_to_source_ptr); + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(*tf_target_to_source_ptr); + tier4_autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); tier4_autoware_utils::transformPointCloud( @@ -731,6 +739,33 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } +int NDTScanMatcher::count_oscillation( + const std::vector & result_pose_msg_array) +{ + constexpr double inversion_vector_threshold = -0.9; + + int oscillation_cnt = 0; + int max_oscillation_cnt = 0; + + for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { + const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); + const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); + const Eigen::Vector3d prev_prev_pose = + point_to_vector3d(result_pose_msg_array.at(i - 2).position); + const auto current_vec = (current_pose - prev_pose).normalized(); + const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); + const double cosine_value = current_vec.dot(prev_vec); + const bool oscillation = cosine_value < inversion_vector_threshold; + if (oscillation) { + oscillation_cnt++; // count consecutive oscillation + } else { + oscillation_cnt = 0; // reset + } + max_oscillation_cnt = std::max(max_oscillation_cnt, oscillation_cnt); + } + return max_oscillation_cnt; +} + std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) @@ -835,17 +870,29 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - auto tf_pose_to_map_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - get_clock()->now(), map_frame_, req->pose_with_covariance.header.frame_id, tf_pose_to_map_ptr); + const std::string & target_frame = map_frame_; + const std::string & source_frame = req->pose_with_covariance.header.frame_id; - // transform pose_frame to map_frame - const auto initial_pose_msg_in_map_frame = - transform(req->pose_with_covariance, *tf_pose_to_map_ptr); - if (use_dynamic_map_loading_) { - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + geometry_msgs::msg::TransformStamped transform_s2t; + try { + transform_s2t = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + // Note: Up to AWSIMv1.1.0, there is a known bug where the GNSS frame_id is incorrectly set to + // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue + // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + transform_s2t.header.stamp = get_clock()->now(); + transform_s2t.header.frame_id = target_frame; + transform_s2t.child_frame_id = source_frame; + transform_s2t.transform = tf2::toMsg(tf2::Transform::getIdentity()); } + // transform pose_frame to map_frame + const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + // mutex Map std::lock_guard lock(ndt_ptr_mtx_); diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index f50eba9218d67..2dc864b0cb925 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -7,6 +7,7 @@ Takagi, Isamu Yamato Ando Masahiro Sakamoto + Kento Yabuuchi Apache License 2.0 Takagi, Isamu diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 1eb1edd04a68a..a05c63d44ebce 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -101,11 +101,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) req->area.center_y = point.y; req->area.radius = 50; - RCLCPP_INFO(logger, "Send request to map_loader"); + RCLCPP_DEBUG(logger, "Send request to map_loader"); auto future = cli_map_->async_send_request(req); auto status = future.wait_for(std::chrono::seconds(1)); while (status != std::future_status::ready) { - RCLCPP_INFO(logger, "waiting response"); + RCLCPP_DEBUG(logger, "waiting response"); if (!rclcpp::ok()) { return false; } @@ -113,7 +113,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto res = future.get(); - RCLCPP_INFO( + RCLCPP_DEBUG( logger, "Loaded partial pcd map from map_loader (grid size: %lu)", res->new_pointcloud_with_ids.size()); @@ -168,7 +168,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st const auto logger = node_->get_logger(); tf2::Vector3 point(position.x, position.y, position.z); - RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); if (cli_map_) { if (!get_partial_point_cloud_map(position)) { @@ -193,7 +193,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st return std::nullopt; } - RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); Point result; result.x = point.getX(); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 65d9594cb7415..fbe019096a3e7 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma #### Prerequisites on pointcloud map file(s) -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. @@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. -Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). - #### Metadata structure The metadata should look like this: @@ -118,7 +116,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | enable_selected_load | bool | A flag to enable selected pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | | pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ba4c032d3e514..b4efbec9706b4 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,7 +3,6 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 5230d4bd03214..c8fcce6f7002f 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -8,6 +8,7 @@ Ryu Yamamoto Koji Minoda Masahiro Sakamoto + Kento Yabuuchi Apache License 2.0 diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp new file mode 100644 index 0000000000000..225445d17bfa1 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp @@ -0,0 +1,41 @@ +// 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. + +#ifndef LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ +#define LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ + +#include + +namespace lanelet::projection +{ + +class LocalProjector : public Projector +{ +public: + LocalProjector() : Projector(Origin(GPSPoint{})) {} + + BasicPoint3d forward(const GPSPoint & gps) const override // NOLINT + { + return BasicPoint3d{0.0, 0.0, gps.ele}; + } + + GPSPoint reverse(const BasicPoint3d & point) const override + { + return GPSPoint{0.0, 0.0, point.z()}; + } +}; + +} // namespace lanelet::projection + +#endif // LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 259c168edcc5c..5db00fe0ad2f5 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -33,6 +33,8 @@ #include "map_loader/lanelet2_map_loader_node.hpp" +#include "lanelet2_local_projector.hpp" + #include #include #include @@ -100,8 +102,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return map; } } else { - // Use MGRSProjector as parser - lanelet::projection::MGRSProjector projector{}; + const lanelet::projection::LocalProjector projector; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); // overwrite local_x, local_y diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 082dc95d6500a..a0c57759d51a6 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -215,6 +215,12 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); insertMarkerArray( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); @@ -224,6 +230,9 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index a2d9307084545..5f4b3e311e6e9 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); bool enable_selected_load = declare_parameter("enable_selected_load"); if (enable_whole_load) { @@ -68,26 +67,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load || enable_selected_load) { - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict); - } + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_selected_load) { + selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); } } diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 16442eb43c740..54e794e2742bf 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -23,6 +23,8 @@ #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename); class MapProjectionLoader : public rclcpp::Node { diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b128c2cf15e15..76e40d4379de4 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -7,6 +7,7 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi Apache License 2.0 ament_cmake_auto diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9fdba288601cc..5966baaed8383 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -20,6 +20,7 @@ #include +#include #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) @@ -55,28 +56,40 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) { - std::string yaml_filename = this->declare_parameter("map_projector_info_path"); - std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); - std::ifstream file(yaml_filename); - tier4_map_msgs::msg::MapProjectorInfo msg; - bool use_yaml_file = file.is_open(); - if (use_yaml_file) { - RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + if (std::filesystem::exists(yaml_filename)) { + std::cout << "Load " << yaml_filename << std::endl; msg = load_info_from_yaml(yaml_filename); - } else { - RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); - RCLCPP_WARN( - this->get_logger(), - "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead. For more info, visit " - "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" - "README.md"); + } else if (std::filesystem::exists(lanelet2_map_filename)) { + std::cout << "Load " << lanelet2_map_filename << std::endl; + std::cout + << "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader/" + "README.md" + << std::endl; msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } else { + throw std::runtime_error( + "No map projector info files found. Please provide either " + "map_projector_info.yaml or lanelet2_map.osm"); } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + const std::string lanelet2_map_filename = + this->declare_parameter("lanelet2_map_path"); + + const tier4_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message const auto adaptor = component_interface_utils::NodeAdaptor(this); diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 6dc68911699aa..6e266b0ad82f0 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -4,7 +4,9 @@ map_tf_generator 0.1.0 map_tf_generator package as a ROS 2 node - azumi-suzuki + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_auto diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 4655cf4c2f73a..d56cb3fb31584 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -89,6 +89,8 @@ install( RUNTIME DESTINATION bin ) -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch + config ) diff --git a/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml new file mode 100644 index 0000000000000..3d5270d17479e --- /dev/null +++ b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + map_layer_name: "elevation" + height_diff_thresh: 0.15 + map_frame: "map" diff --git a/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..f95c0b0c94b92 --- /dev/null +++ b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml index 42478c514b788..9cb41c8780f13 100644 --- a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml index 4a272646503f1..cdf17ebfeda9f 100644 --- a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml index 2942781ce8123..e3f4594729c2b 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index 2fc2884fd5df9..01ad5cd9b1792 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -1,25 +1,16 @@ + + - - - - - - - - - - - - + diff --git a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml index 09ea9022b97e5..13023a8c1c2c9 100644 --- a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json new file mode 100644 index 0000000000000..24ac2481a4e4a --- /dev/null +++ b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Compare Elevation Map Filter", + "type": "object", + "definitions": { + "compare_elevation_map_filter": { + "type": "object", + "properties": { + "map_layer_name": { + "type": "string", + "default": "elevation", + "description": "elevation map layer name" + }, + "height_diff_thresh": { + "type": "number", + "default": "0.15", + "description": "Remove points whose height difference is below this value [m]" + }, + "map_frame": { + "type": "string", + "default": "map", + "description": "frame_id of the map that is temporarily used before elevation_map is subscribed" + } + }, + "required": ["map_layer_name", "height_diff_thresh", "map_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/compare_elevation_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..10454b48d3d80 --- /dev/null +++ b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Distance Based Compare Map Filter", + "type": "object", + "definitions": { + "distance_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/distance_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json new file mode 100644 index 0000000000000..3c301d5ad9fbb --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Based Approximate Compare Map Filter", + "type": "object", + "definitions": { + "voxel_based_approximate_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_based_approximate_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..2e541662a1743 --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Based Compare Map Filter", + "type": "object", + "definitions": { + "voxel_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + }, + "map_update_distance_threshold": { + "type": "number", + "default": "10.0", + "description": "Threshold distance to update map points with input points [m]" + }, + "map_loader_radius": { + "type": "number", + "default": "150.0", + "description": "Radius to load map points [m]" + }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + } + }, + "required": [ + "distance_threshold", + "use_dynamic_map_loading", + "downsize_ratio_z_axis", + "map_update_distance_threshold", + "map_loader_radius", + "timer_interval_ms" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..4663dbe806223 --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Distance Based Compare Map Filter", + "type": "object", + "definitions": { + "voxel_distance_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_distance_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp index 7c8f2e8d4bd76..1eadeeb3bec05 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp @@ -40,9 +40,9 @@ CompareElevationMapFilterComponent::CompareElevationMapFilterComponent( : Filter("CompareElevationMapFilter", options) { unsubscribe(); - layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); - height_diff_thresh_ = this->declare_parameter("height_diff_thresh", 0.15); - map_frame_ = this->declare_parameter("map_frame", "map"); + layer_name_ = declare_parameter("map_layer_name"); + height_diff_thresh_ = declare_parameter("height_diff_thresh"); + map_frame_ = declare_parameter("map_frame"); rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml index 33168c87de7eb..2be9074f8d71e 100644 --- a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml +++ b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml @@ -16,3 +16,4 @@ ros__parameters: use_last_detect_color: true last_detect_color_hold_time: 2.0 + last_colors_hold_time: 1.0 diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 848293f6334bb..0578d671d27b9 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -34,11 +34,11 @@ #include #include +#include #include #include #include #include - namespace traffic_light { @@ -53,6 +53,8 @@ using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement using TrafficSignalAndTime = std::pair; using TrafficLightIdMap = std::unordered_map; +using TrafficLightIdArray = std::unordered_map>; + class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node { public: @@ -76,8 +78,12 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg); void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals); + void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals); + void updateFlashingState(const TrafficSignal & signal); + uint8_t updateAndGetColorState(const TrafficSignal & signal); void setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const; + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output); lanelet::ConstLanelets getNonRedLanelets( const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const; @@ -97,9 +103,15 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; + double last_colors_hold_time_; // Signal history TrafficLightIdMap last_detect_color_; + TrafficLightIdArray last_colors_; + + // State + std::map is_flashing_; + std::map current_color_state_; // Stop watch StopWatch stop_watch_; diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 6527f0662bcbf..904a365786755 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -73,7 +73,6 @@ bool hasMergeLane( return false; } - } // namespace CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( @@ -84,6 +83,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( use_last_detect_color_ = declare_parameter("use_last_detect_color"); last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); + last_colors_hold_time_ = declare_parameter("last_colors_hold_time"); sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), @@ -164,19 +164,19 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( traffic_light_id_map[traffic_signal.traffic_signal_id] = std::pair(traffic_signal, get_clock()->now()); } - for (const auto & crosswalk : conflicting_crosswalks_) { constexpr int VEHICLE_GRAPH_ID = 0; const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); const auto non_red_lanelets = getNonRedLanelets(conflict_lls, traffic_light_id_map); const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, non_red_lanelets); - setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output); + setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, *msg, output); } removeDuplicateIds(output); updateLastDetectedSignal(traffic_light_id_map); + updateLastDetectedSignals(traffic_light_id_map); pub_traffic_light_array_->publish(output); pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total")); @@ -221,24 +221,160 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( } } } - for (const auto id : erase_id_list) last_detect_color_.erase(id); + for (const auto id : erase_id_list) { + last_detect_color_.erase(id); + is_flashing_.erase(id); + current_color_state_.erase(id); + } +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if ( + elements.front().color == TrafficSignalElement::UNKNOWN && elements.front().confidence == 1) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_signal_id; + + if (last_colors_.count(id) == 0) { + std::vector signal{input_traffic_signal.second}; + last_colors_.insert(std::make_pair(id, signal)); + continue; + } + + last_colors_.at(id).push_back(input_traffic_signal.second); + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_colors_) { + const auto & id = last_traffic_signal.first; + for (auto it = last_traffic_signal.second.begin(); it != last_traffic_signal.second.end();) { + auto sig = (*it).first; + rclcpp::Time t = (*it).second; + + // hold signal recognition results for [last_colors_hold_time_] seconds. + const auto time_from_last_detected = (get_clock()->now() - t).seconds(); + if (time_from_last_detected > last_colors_hold_time_) { + it = last_traffic_signal.second.erase(it); + } else { + ++it; + } + } + if (last_traffic_signal.second.empty()) { + erase_id_list.emplace_back(id); + } + } + for (const auto id : erase_id_list) last_colors_.erase(id); } void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output) { const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); + std::unordered_map valid_id2idx_map; // detected traffic light + + for (size_t i = 0; i < msg.signals.size(); ++i) { + auto signal = msg.signals[i]; + valid_id2idx_map[signal.traffic_signal_id] = i; + } + for (const auto & tl_reg_elem : tl_reg_elems) { - 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(); - msg.signals.push_back(output_traffic_signal); + auto id = tl_reg_elem->id(); + // if valid prediction exists, overwrite the estimation; else, use the estimation + if (valid_id2idx_map.count(id)) { + size_t idx = valid_id2idx_map[id]; + auto signal = msg.signals[idx]; + updateFlashingState(signal); // check if it is flashing + // update output msg according to flashing and current state + output.signals[idx].elements[0].color = updateAndGetColorState(signal); + } else { + 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 = id; + output.signals.push_back(output_traffic_signal); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_signal_id; + + // no record of detected color in last_detect_color_hold_time_ + if (is_flashing_.count(id) == 0) { + is_flashing_.insert(std::make_pair(id, false)); + return; + } + + // flashing green + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence != 0 && // not due to occlusion + current_color_state_.at(id) != TrafficSignalElement::UNKNOWN) { + is_flashing_.at(id) = true; + return; + } + + // history exists + if (last_colors_.count(id) > 0) { + std::vector history = last_colors_.at(id); + for (const auto & h : history) { + if (h.first.elements.front().color != signal.elements.front().color) { + // keep the current value if not same with input signal + return; + } + } + // all history is same with input signal + is_flashing_.at(id) = false; + } + + // no record of detected color in last_color_hold_time_ + // keep the current value + return; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_signal_id; + const auto color = signal.elements[0].color; + + if (current_color_state_.count(id) == 0) { + current_color_state_.insert(std::make_pair(id, color)); + } else if (is_flashing_.at(id) == false) { + current_color_state_.at(id) = color; + } else if (is_flashing_.at(id) == true) { + if ( + current_color_state_.at(id) == TrafficSignalElement::GREEN && + color == TrafficSignalElement::RED) { + current_color_state_.at(id) = TrafficSignalElement::RED; + } else if ( + current_color_state_.at(id) == TrafficSignalElement::RED && + color == TrafficSignalElement::GREEN) { + current_color_state_.at(id) = TrafficSignalElement::GREEN; + } else if (current_color_state_.at(id) == TrafficSignalElement::UNKNOWN) { + if (color == TrafficSignalElement::GREEN || color == TrafficSignalElement::UNKNOWN) + current_color_state_.at(id) = TrafficSignalElement::GREEN; + if (color == TrafficSignalElement::RED) + current_color_state_.at(id) = TrafficSignalElement::RED; + } } + + return current_color_state_.at(id); } lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp similarity index 88% rename from perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index ae6162542a1c3..4331b5c19d3f1 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include #include @@ -69,4 +69,4 @@ class ObjectLaneletFilterNode : public rclcpp::Node } // namespace object_lanelet_filter -#endif // DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp similarity index 84% rename from perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index b9384e0f70379..be25eb6a353e6 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include #include @@ -55,4 +55,4 @@ class ObjectPositionFilterNode : public rclcpp::Node } // namespace object_position_filter -#endif // DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp similarity index 93% rename from perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp index 6597475ae297e..8f5fa054090f5 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ #include @@ -113,4 +113,4 @@ class Debugger }; } // namespace obstacle_pointcloud_based_validator -#endif // OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp similarity index 90% rename from perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index e7df2c409b18f..2d70247445043 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ -#define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ -#include "obstacle_pointcloud_based_validator/debugger.hpp" +#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" #include @@ -152,4 +154,5 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node }; } // namespace obstacle_pointcloud_based_validator -#endif // OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp similarity index 89% rename from perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index f62b0d866dc3a..5fd866d09e725 100644 --- a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ #include #include @@ -68,4 +68,4 @@ class OccupancyGridBasedValidator : public rclcpp::Node }; } // namespace occupancy_grid_based_validator -#endif // OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/include/utils/utils.hpp b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp similarity index 84% rename from perception/detected_object_validation/include/utils/utils.hpp rename to perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp index 21df77825176b..2c46e1b9dd110 100644 --- a/perception/detected_object_validation/include/utils/utils.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include @@ -33,4 +33,4 @@ struct FilterTargetLabel }; // struct FilterTargetLabel } // namespace utils -#endif // UTILS__UTILS_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index fe8de37c778ac..cd16d414425cb 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_filter/object_lanelet_filter.hpp" +#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" #include #include diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 0f59e60d57d55..a509fc7571dd5 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_filter/object_position_filter.hpp" +#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" namespace object_position_filter { 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 3249581513dd9..d903a9ca3bb41 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" #include #include diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 948f040d7ebde..80e4dc66d35c2 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" #include #include diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/src/utils.cpp index d8f438e265c3a..53b21ee7ff5b6 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 10affd0b94ffd..771747c80bb95 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -16,6 +16,7 @@ #define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ #include "detection_by_tracker/debugger.hpp" +#include "detection_by_tracker/utils.hpp" #include #include @@ -39,8 +40,6 @@ #include #endif -#include "utils/utils.hpp" - #include #include @@ -82,7 +81,7 @@ class DetectionByTracker : public rclcpp::Node std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - utils::TrackerIgnoreLabel tracker_ignore_; + detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/include/utils/utils.hpp b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp similarity index 81% rename from perception/detection_by_tracker/include/utils/utils.hpp rename to perception/detection_by_tracker/include/detection_by_tracker/utils.hpp index 3f39125b95e03..ed783b0343a14 100644 --- a/perception/detection_by_tracker/include/utils/utils.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef DETECTION_BY_TRACKER__UTILS_HPP_ +#define DETECTION_BY_TRACKER__UTILS_HPP_ #include +namespace detection_by_tracker +{ namespace utils { struct TrackerIgnoreLabel @@ -32,5 +34,6 @@ struct TrackerIgnoreLabel bool isIgnore(const uint8_t label) const; }; // struct TrackerIgnoreLabel } // namespace utils +} // namespace detection_by_tracker -#endif // UTILS__UTILS_HPP_ +#endif // DETECTION_BY_TRACKER__UTILS_HPP_ diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp index 29a500a24cf32..b2655d99abacc 100644 --- a/perception/detection_by_tracker/src/utils.cpp +++ b/perception/detection_by_tracker/src/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "detection_by_tracker/utils.hpp" #include +namespace detection_by_tracker +{ namespace utils { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -28,3 +30,4 @@ bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } } // namespace utils +} // namespace detection_by_tracker diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index c021dd92dae64..b13f68b07181e 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/segmentation_pointcloud_fusion/node.cpp src/roi_pointcloud_fusion/node.cpp ) @@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode" + EXECUTABLE segmentation_pointcloud_fusion_node +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" EXECUTABLE roi_pointcloud_fusion_node diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index e1be5426cba4b..21d31f216373b 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,18 +1,26 @@ /**: ros__parameters: - class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] - paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] - point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle - max_voxel_size: 40000 - point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] - voxel_size: [0.32, 0.32, 8.0] - downsample_factor: 1 - encoder_in_feature_size: 12 - yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] - # post-process params - circle_nms_dist_threshold: 0.3 - iou_nms_target_class_names: ["CAR"] - iou_nms_search_distance_2d: 10.0 - iou_nms_threshold: 0.1 - # omp params - omp_num_threads: 1 + model_params: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle + max_voxel_size: 40000 + point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + voxel_size: [0.32, 0.32, 8.0] + downsample_factor: 1 + encoder_in_feature_size: 12 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + has_twist: false + densification_params: + world_frame_id: "map" + num_past_frames: 0 + post_process_params: + # post-process params + circle_nms_dist_threshold: 0.3 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + score_threshold: 0.4 + omp_params: + # omp params + num_threads: 1 diff --git a/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml new file mode 100644 index 0000000000000..90ba841d53b2d --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + fusion_distance: 100.0 + trust_object_distance: 100.0 + trust_object_iou_mode: "iou" + non_trust_object_iou_mode: "iou_x" + use_cluster_semantic_type: false + only_allow_inside_cluster: true + roi_scale_factor: 1.1 + iou_threshold: 0.65 + unknown_iou_threshold: 0.1 + remove_unknown: true diff --git a/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..5b86b8e81d1aa --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + fuse_unknown_only: true + min_cluster_size: 2 + cluster_2d_tolerance: 0.5 diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml index 21ba13787f1c0..99d85089befb8 100644 --- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml +++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml @@ -3,3 +3,11 @@ input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] timeout_ms: 70.0 match_threshold_ms: 50.0 + image_buffer_size: 15 + debug_mode: false + filter_scope_min_x: -100.0 + filter_scope_min_y: -100.0 + filter_scope_min_z: -100.0 + filter_scope_max_x: 100.0 + filter_scope_max_y: 100.0 + filter_scope_max_z: 100.0 diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..914ad13519807 --- /dev/null +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: + [ + true, # road + true, # sidewalk + true, # building + true, # wall + true, # fence + true, # pole + true, # traffic_light + true, # traffic_sign + true, # vegetation + true, # terrain + true, # sky + false, # person + false, # ride + false, # car + false, # truck + false, # bus + false, # train + false, # motorcycle + false, # bicycle + false, # others + ] + # the maximum distance of pointcloud to be applied filter, + # this is selected based on semantic segmentation model accuracy, + # calibration accuracy and unknown reaction distance + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg new file mode 100644 index 0000000000000..aaadfaf186dd2 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg @@ -0,0 +1,966 @@ + + + + + + + + + + +
+
+
+ + Check whether + the cluster RoI (inner) + is fully contained within + the scaled detected RoI (outer) + . +
+
+ *NOTE* +
+ Because the cluster RoI is calculated with the points projected on the image, our recommended scale factor is [1.1, 1.5] . +
+ Otherwise, set + only_allow_inside_cluster + to + False + . +
+ +
+
+
+ + Check whether the cluster RoI (inner) is fully contained within the scaled... + + + + + + +
+
+
+ use_iou +
+
+
+ + use_iou + + + + + + +
+
+
+ unknown_iou_threshold +
+
+
+ + unknown_iou_threshold + + + + + + +
+
+
+ + use_iou_x /  y +
+ +
+
+
+ + use_iou_x /  y + + + + + + + + +
+
+
+ only_allow_inside_cluster +
+
+
+ + only_allow_inside_cluster + + + + + + +
+
+
+ iou_threshold +
+
+
+ + iou_threshold + + + + + + + + + + +
+
+
+ Is UNKNOWN object? +
+
+
+ + Is UNKNOWN object? + + + + + + + + +
+
+
+ + IoU threshold < IoU score? + +
+
+
+ + IoU threshold < IoU score? + + + + + + + + + + + + + + +
+
+
+ IoU Score +
+
+
+ + IoU Score + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Is Cluster RoI inside Detected RoI? +
+
+
+ + Is Cluster RoI inside Detected RoI? + + + + + + + + +
+
+
+ Fused Cluster +
+
+
+ + Fused Cluster + + + + + + + + +
+
+
+ Unfused Cluster +
+
+
+ + Unfused Cluster + + + + + + + + + + +
+
+
+ Fusion Target Cluster
&
Detected RoI +
+
+
+ + Fusion Target Cluster... + + + + + + +
+
+
+ Unfused Cluster +
+
+
+ + Unfused Cluster + + + + + + + + + + + + + +
+
+
+ + + roi_scale_factor
(>=1.0) + + +
+
+
+ + roi_scale_factor... + + + + + + +
+
+
+ + Check whether the IoU score between + the cluster RoI + and + the detected RoI + is greater than the threshold. +
+
+ **NOTE** +
+ Our default threshold is 0.65 with + only_allow_inside_cluster + is + True + . +
+ Also, please try the bigger score threshold and set + only_allow_inside_cluster + to + False + depending on your 2D RoI detector performance. +
+ +
+
+
+ + Check whether the IoU score between the cluster RoI and the detected RoI is greater than the thr... + + + + + + + + +
+
+
+
    +
  • + IoU = Intersection Area / Union Area +
    • +
  • + IoU X = Intersection Width / Union Width +
    • +
  • + IoU Y = Intersection Height / Union Height +
    • +
+
+
+
+ + IoU = Intersection Area / Union AreaIoU X = Intersectio... + + + + + + +
+
+
+ + Nodes overview + +
+
+
+ + Nodes overview + + + + + + +
+
+
+ Numeric Parameter +
+
+
+ + Numeric Parameter + + + + + + +
+
+
+ Result +
+
+
+ + Result + + + + + + + + + + +
+
+
+ Boolean Parameter +
+
+
+ + Boolean Parameter + + + + + + +
+
+
+ + true + +
+
+
+ + true + + + + + + +
+
+
+ + false + +
+
+
+ + false + + + + + + + + + + +
+
+
+ Condition +
+
+
+ + Condition + + + + + + +
+
+
+ + true + +
+
+
+ + true + + + + + + +
+
+
+ + false + +
+
+
+ + false + + + + + + +
+
+
+ use_cluster_semantic_type +
+
+
+ + use_cluster_semantic_type + + + + + + +
+
+
+ + Objects +
+ w/ Semantic Label + +
+
+
+ + Objects... + + + + + + + + +
+
+
+ + All objects are set to +
+ UNKOWN Label +
+ +
+
+
+ + All objects are set to... + + + + + + + + + + +
+
+
+ Fusion Target Cluster +
+
+
+ + Fusion Target Cluster + + + + + + +
+
+
+ trust_distance +
+
+
+ + trust_distance + + + + + + + + +
+
+
+ + Is Cluster closer than + trust_distance + ? + +
+
+
+ + Is Cluster closer than trust_... + + + + + + + + + + + + + + +
+
+
+ + Input Cluster + +
+
+
+ + Input Cluster + + + + + + +
+
+
+ roi_scale_factor +
+
+
+ + roi_scale_factor + + + + + + +
+
+
+ Pre-Process +
+
+
+ + Pre-Process + + + + + + +
+
+
+ Fusion Process +
+
+
+ + Fusion Process + + + + + + +
+
+
+ Post-Process +
+
+
+ + Post-Process + + + + + + + + +
+
+
+ remove_unknown +
+
+
+ + remove_unknown + + + + + + + + +
+
+
+ Fused Cluster +
+
+
+ + Fused Cluster + + + + + + +
+
+
+ Is KNOWN object?
&&
0.1 <= RoI probability?
 +
+
+
+ + Is KNOWN object?... + + + + + + + + +
+
+
+ Remove noise clusters, which are undetected by RoI detector, such as fog and exhaust gas. +
+
+
+ + Remove noise clusters, which are undet... + + + + + + +
+
+
+ + Output Fused Cluster + +
+
+
+ + Output Fused Cluster + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + 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 03eaab2a3c6ca..86d3a2fa070b2 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -30,6 +30,11 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ## Parameters +The following figure is an inner pipeline overview of RoI cluster fusion node. +Please refer to it for your parameter settings. + +![roi_cluster_fusion_pipeline](./images/roi_cluster_fusion_pipeline.svg) + ### Core Parameters | Name | Type | Description | diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md new file mode 100644 index 0000000000000..77188aafb3b22 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -0,0 +1,87 @@ +# segmentation_pointcloud_fusion + +## Purpose + +The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud that are belong to less interesting region which is defined by semantic or instance segmentation by 2D image segmentation model. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network. +- The pointclouds are belong to segment such as road, sidewalk, building, vegetation, sky ... are considered as less important points for autonomous driving and could be removed. + +![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | ----------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| -------- | ------------------------------- | -------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ------------- | ---- | ------------------------ | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 9572e62e0e0aa..fe8acb6746dba 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -49,13 +50,14 @@ namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::CameraInfo; +using sensor_msgs::msg::Image; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware_auto_perception_msgs::msg::ObjectClassification; - -template +template class FusionNode : public rclcpp::Node { public: @@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node virtual void subCallback(const typename Msg::ConstSharedPtr input_msg); // callback for roi subscription + virtual void roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); virtual void fuseOnSingleImage( - const Msg & input_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, + const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0; // set args if you need @@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; - std::vector::SharedPtr> rois_subs_; + std::vector::SharedPtr> rois_subs_; // offsets between cameras and the lidars std::vector input_offset_ms_; @@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node std::vector is_fused_; std::pair cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + std::vector> cached_roi_msgs_; std::mutex mutex_cached_msgs_; // output publisher diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index d7de10fed068f..78ae152141a00 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,7 +33,8 @@ namespace image_projection_based_fusion { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -class PointPaintingFusionNode : public FusionNode +class PointPaintingFusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); 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 e54710ad477da..b9471f2f3b78e 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 @@ -25,7 +25,8 @@ namespace image_projection_based_fusion const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode -: public FusionNode +: public FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index e11a62c060480..7d7132309fb91 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -29,7 +29,8 @@ namespace image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode : public FusionNode +class RoiDetectedObjectFusionNode +: public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2b4eb822a9edb..fe685baa0b6e2 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -22,7 +22,7 @@ namespace image_projection_based_fusion { class RoiPointCloudFusionNode -: public FusionNode +: public FusionNode { private: int min_cluster_size_{1}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..4168c483469ab --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace image_projection_based_fusion +{ +class SegmentPointCloudFusionNode : public FusionNode +{ +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + std::vector filter_semantic_label_target_; + float filter_distance_threshold_; + /* data */ +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(PointCloud2 & pointcloud_msg) override; + + void postprocess(PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, + const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + + bool out_of_scope(const PointCloud2 & filtered_cloud); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index e15737f5ed222..33781461fa1cc 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -30,7 +30,6 @@ - @@ -43,9 +42,6 @@ - - - @@ -82,9 +78,6 @@ - - - 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 60f6f943b8cda..52dd71e9579c1 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 @@ -18,24 +18,11 @@ + - - - - - - - - - - - - - - @@ -46,17 +33,8 @@ - - - - - - - - - - + @@ -86,16 +64,6 @@ - - - - - - - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index b6165fc7c87d2..c9da81af9ddb0 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -25,8 +25,6 @@ - - @@ -69,10 +67,6 @@ - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index 181f4ccb88320..046d88d06e2a1 100644 --- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -1,8 +1,5 @@ - - - @@ -23,6 +20,7 @@ + @@ -38,9 +36,7 @@ - - - + diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..96bf47594bfe8 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 1648de210ec2c..49ff4dafc7900 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -9,6 +9,8 @@ Shunsuke Miura Yoshi Ri badai nguyen + Kotaro Uetake + Tao Zhong Apache License 2.0 ament_cmake_auto diff --git a/perception/image_projection_based_fusion/schema/pointpainting.schema.json b/perception/image_projection_based_fusion/schema/pointpainting.schema.json new file mode 100644 index 0000000000000..036628d72e70a --- /dev/null +++ b/perception/image_projection_based_fusion/schema/pointpainting.schema.json @@ -0,0 +1,152 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Point Painting Fusion Node", + "type": "object", + "definitions": { + "pointpainting": { + "type": "object", + "properties": { + "model_params": { + "type": "object", + "description": "Parameters for model configuration.", + "properties": { + "class_names": { + "type": "array", + "description": "An array of class names will be predicted.", + "default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "paint_class_names": { + "type": "array", + "description": "An array of class names will be painted by PointPainting", + "default": ["CAR", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "point_feature_size": { + "type": "integer", + "description": "A number of channels of point feature layer.", + "default": 7 + }, + "max_voxel_size": { + "type": "integer", + "description": "A maximum size of voxel grid.", + "default": 40000 + }, + "point_cloud_range": { + "type": "array", + "description": "An array of distance ranges of each class, this must have same length with `class_names`.", + "default": [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + }, + "voxel_size": { + "type": "array", + "description": "An array of voxel grid sizes for PointPainting, this must have same length with `paint_class_names`.", + "default": [0.32, 0.32, 8.0] + }, + "down_sample_factor": { + "type": "integer", + "description": "A scale factor of downsampling points", + "default": 1, + "minimum": 1 + }, + "encoder_in_feature_size": { + "type": "integer", + "description": "A size of encoder input feature channels.", + "default": 12 + }, + "yaw_norm_thresholds": { + "type": "array", + "description": "An array of distance threshold values of norm of yaw [rad].", + "default": [0.3, 0.3, 0.3, 0.3, 0.0], + "minimum": 0.0, + "maximum": 1.0 + }, + "has_twist": { + "type": "boolean", + "description": "Indicates whether the model outputs twist value.", + "default": false + } + } + }, + "densification_params": { + "type": "object", + "description": "Parameters for pointcloud densification.", + "properties": { + "world_frame_id": { + "type": "string", + "description": "A name of frame id where world coordinates system is defined with respect to.", + "default": "map" + }, + "num_past_frames": { + "type": "integer", + "description": "A number of past frames to be considered as same input frame.", + "default": 0, + "minimum": 0 + } + } + }, + "post_process_params": { + "type": "object", + "properties": { + "score_threshold": { + "type": "number", + "description": "A threshold value of existence probability score, all of objects with score less than this threshold are ignored.", + "default": 0.4, + "minimum": 0.0, + "maximum": 1.0 + }, + "circle_nms_dist_threshold": { + "type": "number", + "description": "", + "default": 0.3, + "minimum": 0.0, + "maximum": 1.0 + }, + "iou_nms_target_class_names": { + "type": "array", + "description": "An array of class names to be target in NMS.", + "default": ["CAR"], + "uniqueItems": true + }, + "iou_search_distance_2d": { + "type": "number", + "description": "A maximum distance value to search the nearest objects.", + "default": 10.0, + "minimum": 0.0 + }, + "iou_nms_threshold": { + "type": "number", + "description": "A threshold value of NMS using IoU score.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + } + } + }, + "omp_params": { + "type": "object", + "properties": { + "num_threads": { + "type": "integer", + "description": "The number of threads that is set to the environment variable OMP_NUM_THREADS.", + "default": 1, + "minimum": 1 + } + } + } + }, + "required": ["model_params", "densification_params", "post_process_params", "omp_params"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pointpainting" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json new file mode 100644 index 0000000000000..fc32e9d6d3d8b --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json @@ -0,0 +1,96 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI Cluster Fusion Node", + "type": "object", + "definitions": { + "roi_cluster_fusion": { + "type": "object", + "properties": { + "fusion_distance": { + "type": "number", + "description": "If the detected object's distance is less than its value, the fusion will be processed.", + "default": 100.0, + "minimum": 0.0 + }, + "trust_object_distance": { + "type": "number", + "description": "If the detected object's distance is less than its value, IoU method specified in `trust_object_iou_mode` is used, otherwise `non_trust_object_iou_mode` is used.", + "default": 100.0, + "minimum": 0.0 + }, + "trust_object_iou_mode": { + "type": "string", + "description": "Name of IoU method applied to the objects in range of [0.0, `trust_distance`].", + "default": "iou", + "enum": ["iou", "iou_x", "iou_y"] + }, + "non_trust_object_iou_mode": { + "type": "string", + "description": "Name of IoU method applied to the objects in range of [`trust_distance`, `fusion_distance`], if `trust_distance` < `fusion_distance`.", + "default": "iou_x", + "enum": ["iou", "iou_x", "iou_y"] + }, + "use_cluster_semantic_type": { + "type": "boolean", + "description": "If this parameter is false, label of cluster objects will be reset to UNKNOWN.", + "default": false + }, + "only_allow_inside_cluster": { + "type": "boolean", + "description": "If this parameter is true, only clusters in which all their points are inside the RoI can be assigned to the RoI.", + "default": true + }, + "roi_scale_factor": { + "type": "number", + "description": "A scale factor for resizing RoI while checking if cluster points are inside the RoI.", + "default": 1.1, + "minimum": 1.0, + "maximum": 2.0 + }, + "iou_threshold": { + "type": "number", + "description": "An IoU score threshold. Note that the total IoU score is the sum of the IoU scores that are set to true in use_iou, use_iou_x and use_iou_y.", + "default": 0.65, + "minimum": 0.0, + "maximum": 1.0 + }, + "unknown_iou_threshold": { + "type": "number", + "description": "A threshold value of IoU score for objects labeled UNKNOWN.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + }, + "remove_unknown": { + "type": "boolean", + "description": "If this parameter is true, all of objects labeled UNKNOWN will be removed in post-process.", + "default": false + } + }, + "required": [ + "fusion_distance", + "trust_object_distance", + "trust_object_iou_mode", + "non_trust_object_iou_mode", + "use_cluster_semantic_type", + "only_allow_inside_cluster", + "roi_scale_factor", + "iou_threshold", + "unknown_iou_threshold", + "remove_unknown" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_cluster_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json new file mode 100644 index 0000000000000..3030be1305d56 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json @@ -0,0 +1,70 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI Detected Object Fusion Node", + "type": "object", + "definitions": { + "roi_detected_object_fusion": { + "type": "object", + "properties": { + "passthrough_lower_bound_probability_thresholds": { + "type": "array", + "description": "An array of object probability thresholds. The objects that have higher probability than their respective thresholds are kept.", + "default": [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.5] + }, + "trust_distances": { + "type": "array", + "description": "An array of object distances thresholds. Any objects that is farther than this value will be skipped in the clustering process, but will still be published.", + "default": [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + }, + "min_iou_threshold": { + "type": "number", + "description": "An Iou score threshold.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + }, + "roi_probability_threshold": { + "type": "number", + "description": "A object probability threshold.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + }, + "use_roi_probability": { + "type": "boolean", + "description": "If this parameter is true, the objects are filtered out with their RoI probabilities.", + "default": false + }, + "can_assign_matrix": { + "type": "array", + "description": "An NxN matrix, where N represents the number of classes. A value 1 indicates that it is assignable, while a value of 0 indicates not.", + "default": [ + 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, + 0, 0, 0, 1, 1, 1 + ] + } + }, + "required": [ + "passthrough_lower_bound_probability_thresholds", + "trust_distances", + "min_iou_threshold", + "roi_probability_threshold", + "use_roi_probability", + "can_assign_matrix" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_detected_object_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..f39ef257ea789 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json @@ -0,0 +1,41 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI PointCloud Fusion Node", + "type": "object", + "definitions": { + "roi_pointcloud_fusion": { + "type": "object", + "properties": { + "fuse_unknown_only": { + "type": "boolean", + "description": "Whether to fuse only UNKNOWN clusters.", + "default": true + }, + "min_cluster_size": { + "type": "integer", + "description": "The minimum number of points that a cluster must contain to be considered as valid.", + "default": 2 + }, + "cluster_2d_tolerance": { + "type": "number", + "description": "A cluster tolerance measured in radial direction [m]", + "default": 0.5, + "exclusiveMinimum": 0.0 + } + }, + "required": ["fuse_unknown_only", "min_cluster_size", "cluster_2d_tolerance"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_sync.schema.json b/perception/image_projection_based_fusion/schema/roi_sync.schema.json new file mode 100644 index 0000000000000..411fb678a49a7 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_sync.schema.json @@ -0,0 +1,84 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Synchronization of RoI Fusion Nodes", + "type": "object", + "definitions": { + "roi_sync": { + "type": "object", + "properties": { + "input_offset_ms": { + "type": "array", + "description": "An array of timestamp offsets for each camera [ms].", + "default": [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + }, + "timeout_ms": { + "type": "number", + "description": "A timeout value can be assigned within a single frame [ms].", + "default": 70.0, + "minimum": 1.0, + "maximum": 100.0 + }, + "match_threshold_ms": { + "type": "number", + "description": "A maximum threshold value to synchronize RoIs from multiple cameras [ms].", + "default": 50.0, + "minimum": 0.0, + "maximum": 100.0 + }, + "image_buffer_size": { + "type": "integer", + "description": "The number of image buffer size for debug.", + "default": 15, + "minimum": 1 + }, + "debug_mode": { + "type": "boolean", + "description": "Whether to debug or not.", + "default": false + }, + "filter_scope_min_x": { + "type": "number", + "description": "Minimum x position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_min_y": { + "type": "number", + "description": "Minimum y position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_min_z": { + "type": "number", + "description": "Minimum z position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_max_x": { + "type": "number", + "description": "Maximum x position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_max_y": { + "type": "number", + "description": "Maximum y position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_max_z": { + "type": "number", + "description": "Maximum z position [m].", + "default": 100.0 + } + } + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_sync" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index b01a910aaded1..6bced39bc61ef 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -40,13 +40,13 @@ static double processing_time_ms = 0; namespace image_projection_based_fusion { -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number", 1)); + rois_number_ = static_cast(declare_parameter("rois_number")); if (rois_number_ < 1) { RCLCPP_WARN( this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); @@ -80,8 +80,8 @@ FusionNode::FusionNode( "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - input_offset_ms_ = declare_parameter("input_offset_ms", std::vector{}); - if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { + input_offset_ms_ = declare_parameter>("input_offset_ms"); + if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -99,9 +99,9 @@ FusionNode::FusionNode( cached_roi_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function roi_callback = + std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); - rois_subs_.at(roi_i) = this->create_subscription( + rois_subs_.at(roi_i) = this->create_subscription( input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } @@ -122,7 +122,7 @@ FusionNode::FusionNode( // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = - static_cast(declare_parameter("image_buffer_size", 15)); + static_cast(declare_parameter("image_buffer_size")); debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } @@ -136,32 +136,33 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } + // cspell: ignore minx, maxx, miny, maxy, minz, maxz // FIXME: use min_x instead of minx - filter_scope_minx_ = declare_parameter("filter_scope_minx", -100); - filter_scope_maxx_ = declare_parameter("filter_scope_maxx", 100); - filter_scope_miny_ = declare_parameter("filter_scope_miny", -100); - filter_scope_maxy_ = declare_parameter("filter_scope_maxy", 100); - filter_scope_minz_ = declare_parameter("filter_scope_minz", -100); - filter_scope_maxz_ = declare_parameter("filter_scope_maxz", 100); + filter_scope_minx_ = declare_parameter("filter_scope_min_x"); + filter_scope_maxx_ = declare_parameter("filter_scope_max_x"); + filter_scope_miny_ = declare_parameter("filter_scope_min_y"); + filter_scope_maxy_ = declare_parameter("filter_scope_max_y"); + filter_scope_minz_ = declare_parameter("filter_scope_min_z"); + filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { camera_info_map_[camera_id] = *input_camera_info_msg; } -template -void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) +template +void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) +template +void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -284,9 +285,9 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } -template -void FusionNode::roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { stop_watch_ptr_->toc("processing_time", true); @@ -349,14 +350,14 @@ void FusionNode::roiCallback( (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; } -template -void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) +template +void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { using std::chrono_literals::operator""ms; timer_->cancel(); @@ -394,8 +395,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -411,8 +412,8 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const Msg & output_msg) +template +void FusionNode::publish(const Msg & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -420,8 +421,10 @@ void FusionNode::publish(const Msg & output_msg) pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode; -template class FusionNode; -template class FusionNode; +template class FusionNode; +template class FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; +template class FusionNode; +template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 48ef3d26806c8..14783a46ba8b4 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -93,30 +93,32 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("pointpainting_fusion", options) +: FusionNode( + "pointpainting_fusion", options) { - omp_num_threads_ = this->declare_parameter("omp_num_threads", 1); + omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = - static_cast(this->declare_parameter("score_threshold", 0.4)); - const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + static_cast(this->declare_parameter("post_process_params.score_threshold")); + const float circle_nms_dist_threshold = static_cast( + this->declare_parameter("post_process_params.circle_nms_dist_threshold")); const auto yaw_norm_thresholds = - this->declare_parameter>("yaw_norm_thresholds"); + this->declare_parameter>("model_params.yaw_norm_thresholds"); // densification param const std::string densification_world_frame_id = - this->declare_parameter("densification_world_frame_id", "map"); + this->declare_parameter("densification_params.world_frame_id"); const int densification_num_past_frames = - this->declare_parameter("densification_num_past_frames", 0); + this->declare_parameter("densification_params.num_past_frames"); // network param - const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); - const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path", ""); - const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path", ""); - const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); - const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); - - class_names_ = this->declare_parameter>("class_names"); + const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); + const std::string encoder_engine_path = + this->declare_parameter("encoder_engine_path"); + const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); + const std::string head_engine_path = this->declare_parameter("head_engine_path"); + + class_names_ = this->declare_parameter>("model_params.class_names"); const auto paint_class_names = - this->declare_parameter>("paint_class_names"); + this->declare_parameter>("model_params.paint_class_names"); std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; if ( std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") != @@ -138,17 +140,17 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_.erase(cls); } } - has_twist_ = this->declare_parameter("has_twist", false); - const std::size_t point_feature_size = - static_cast(this->declare_parameter("point_feature_size")); + has_twist_ = this->declare_parameter("model_params.has_twist"); + const std::size_t point_feature_size = static_cast( + this->declare_parameter("model_params.point_feature_size")); const std::size_t max_voxel_size = - static_cast(this->declare_parameter("max_voxel_size")); - pointcloud_range = this->declare_parameter>("point_cloud_range"); - const auto voxel_size = this->declare_parameter>("voxel_size"); - const std::size_t downsample_factor = - static_cast(this->declare_parameter("downsample_factor")); - const std::size_t encoder_in_feature_size = - static_cast(this->declare_parameter("encoder_in_feature_size")); + static_cast(this->declare_parameter("model_params.max_voxel_size")); + pointcloud_range = this->declare_parameter>("model_params.point_cloud_range"); + const auto voxel_size = this->declare_parameter>("model_params.voxel_size"); + const std::size_t downsample_factor = static_cast( + this->declare_parameter("model_params.downsample_factor")); + const std::size_t encoder_in_feature_size = static_cast( + this->declare_parameter("model_params.encoder_in_feature_size")); const auto allow_remapping_by_area_matrix = this->declare_parameter>("allow_remapping_by_area_matrix"); const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); @@ -172,10 +174,11 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt { centerpoint::NMSParams p; p.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; - p.target_class_names_ = - this->declare_parameter>("iou_nms_target_class_names"); - p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); - p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + p.target_class_names_ = this->declare_parameter>( + "post_process_params.iou_nms_target_class_names"); + p.search_distance_2d_ = + this->declare_parameter("post_process_params.iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("post_process_params.iou_nms_threshold"); iou_bev_nms_.setParameters(p); } 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 492f832e30e8b..1c064e4d0f060 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 @@ -34,7 +34,8 @@ namespace image_projection_based_fusion { RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_cluster_fusion", options) +: FusionNode( + "roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 333ec7d7ed206..19defc0a1cab0 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion { RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_detected_object_fusion", options) +: FusionNode( + "roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 233e568ebee0b..421aa3a453451 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -30,7 +30,8 @@ namespace image_projection_based_fusion { RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_pointcloud_fusion", options) +: FusionNode( + "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..1c9c865b8d21e --- /dev/null +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -0,0 +1,139 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +namespace image_projection_based_fusion +{ +SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("segmentation_pointcloud_fusion", options) +{ + filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); + filter_semantic_label_target_ = + declare_parameter>("filter_semantic_label_target"); +} + +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} + +void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} +void SegmentPointCloudFusionNode::fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, + __attribute__((unused)) PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + cv_bridge::CvImagePtr in_image_ptr; + try { + in_image_ptr = cv_bridge::toCvCopy( + std::make_shared(input_mask), input_mask.encoding); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what()); + return; + } + + cv::Mat mask = in_image_ptr->image; + if (mask.cols == 0 || mask.rows == 0) { + return; + } + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, + 0.0, 1.0; + geometry_msgs::msg::TransformStamped transform_stamped; + // transform pointcloud from frame id to camera optical frame id + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_mask.header.frame_id, input_pointcloud_msg.header.frame_id, + input_pointcloud_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + PointCloud output_cloud; + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + // skip filtering pointcloud behind the camera or too far from camera + if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + bool is_inside_image = + normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && + normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + if (!is_inside_image) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(normalized_projected_point.y()), + static_cast(normalized_projected_point.x())); + if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + if (!filter_semantic_label_target_.at(semantic_id)) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + + pcl::toROSMsg(output_cloud, output_pointcloud_msg); + output_pointcloud_msg.header = input_pointcloud_msg.header; +} + +bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const PointCloud2 & filtered_cloud) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 6d3d7f5e035a9..ec2f57963332f 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -66,7 +66,7 @@ Lane change logics is illustrated in the figure below.An example of how to tune ### Tuning lane change detection logic -Currently we provide two parameters to tune lane change detection: +Currently we provide three parameters to tune lane change detection: - `dist_threshold_to_bound_`: maximum distance from lane boundary allowed for lane changing vehicle - `time_threshold_to_bound_`: maximum time allowed for lane change vehicle to reach the boundary @@ -124,6 +124,24 @@ See paper [2] for more details. `lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) +#### Pruning predicted paths with lateral acceleration constraint + +It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated. + +Currently we provide three parameters to tune the lateral acceleration constraint: + +- `check_lateral_acceleration_constraints_`: to enable the constraint check. +- `max_lateral_accel_`: max acceptable lateral acceleration for predicted paths (absolute value). +- `min_acceleration_before_curve_`: the minimum acceleration the vehicle would theoretically use to slow down before a curve is taken (must be negative). + +You can change these parameters in rosparam in the table below. + +| param name | default value | +| ----------------------------------------- | -------------- | +| `check_lateral_acceleration_constraints_` | `false` [bool] | +| `max_lateral_accel` | `2.0` [m/s^2] | +| `min_acceleration_before_curve` | `-2.0` [m/s^2] | + ### Path prediction for crosswalk users This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 69ff96a263f4b..fe4d2a51ec6f3 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -13,6 +13,9 @@ sigma_yaw_angle_deg: 5.0 #[angle degree] object_buffer_time_length: 2.0 #[s] history_time_length: 1.0 #[s] + check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints + max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths + min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 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 bdd9ad89bc025..02db91b989116 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 @@ -16,11 +16,15 @@ #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ #include "map_based_prediction/path_generator.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include #include #include +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include #include #include @@ -34,6 +38,7 @@ #include #include +#include #include #include #include @@ -99,9 +104,10 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; - +using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node { public: @@ -123,6 +129,11 @@ class MapBasedPredictionNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + // Pose Transform Listener tier4_autoware_utils::TransformListener transform_listener_{this}; @@ -160,6 +171,10 @@ class MapBasedPredictionNode : public rclcpp::Node bool consider_only_routable_neighbours_; double reference_path_resolution_; + bool check_lateral_acceleration_constraints_; + double max_lateral_accel_; + double min_acceleration_before_curve_; + // Stop watch StopWatch stop_watch_; @@ -237,6 +252,115 @@ class MapBasedPredictionNode : public rclcpp::Node Maneuver predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); + + // NOTE: This function is copied from the motion_velocity_smoother package. + // TODO(someone): Consolidate functions and move them to a common + inline std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist) + { + using tier4_autoware_utils::calcCurvature; + using tier4_autoware_utils::getPoint; + + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } + + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } + + // calculate curvature by circle fitting from three points + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + try { + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN(rclcpp::get_logger("map_based_prediction"), "%s", e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } + } + k_arr.at(i) = curvature; + } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); + + return k_arr; + } + + inline TrajectoryPoints toTrajectoryPoints(const PredictedPath & path, const double velocity) + { + TrajectoryPoints out_trajectory; + std::for_each( + path.path.begin(), path.path.end(), [&out_trajectory, velocity](const auto & pose) { + TrajectoryPoint p; + p.pose = pose; + p.longitudinal_velocity_mps = velocity; + out_trajectory.push_back(p); + }); + return out_trajectory; + }; + + inline bool isLateralAccelerationConstraintSatisfied( + const TrajectoryPoints & trajectory, const double delta_time) + { + constexpr double epsilon = 1E-6; + if (delta_time < epsilon) throw std::invalid_argument("delta_time must be a positive value"); + + if (trajectory.size() < 3) return true; + const double max_lateral_accel_abs = std::fabs(max_lateral_accel_); + + double arc_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + const auto current_pose = trajectory.at(i).pose; + const auto next_pose = trajectory.at(i - 1).pose; + // Compute distance between poses + const double delta_s = std::hypot( + next_pose.position.x - current_pose.position.x, + next_pose.position.y - current_pose.position.y); + arc_length += delta_s; + + // Compute change in heading + tf2::Quaternion q_current, q_next; + tf2::convert(current_pose.orientation, q_current); + tf2::convert(next_pose.orientation, q_next); + double delta_theta = q_current.angleShortestPath(q_next); + // Handle wrap-around + if (delta_theta > M_PI) { + delta_theta -= 2.0 * M_PI; + } else if (delta_theta < -M_PI) { + delta_theta += 2.0 * M_PI; + } + + const double yaw_rate = std::max(std::abs(delta_theta / delta_time), 1.0E-5); + + const double current_speed = std::abs(trajectory.at(i).longitudinal_velocity_mps); + // Compute lateral acceleration + const double lateral_acceleration = std::abs(current_speed * yaw_rate); + if (lateral_acceleration < max_lateral_accel_abs) continue; + const double v_curvature_max = std::sqrt(max_lateral_accel_abs / yaw_rate); + const double t = + (v_curvature_max - current_speed) / min_acceleration_before_curve_; // acc is negative + const double distance_to_slow_down = + current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2); + if (distance_to_slow_down > arc_length) return false; + } + return true; + }; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/schema/map_based_pediction.schema.json b/perception/map_based_prediction/schema/map_based_pediction.schema.json new file mode 100644 index 0000000000000..8ddb122ebb56e --- /dev/null +++ b/perception/map_based_prediction/schema/map_based_pediction.schema.json @@ -0,0 +1,169 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Map Based Prediction", + "type": "object", + "definitions": { + "map_based_prediction": { + "type": "object", + "properties": { + "enable_delay_compensation": { + "type": "boolean", + "default": true, + "description": "flag to enable the time delay compensation for the position of the object" + }, + "prediction_time_horizon": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path" + }, + "lateral_control_time_horizon": { + "type": "number", + "default": "5.0", + "description": "time duration for predicted path will reach the reference path (mostly center of the lane)" + }, + "prediction_sampling_delta_time": { + "type": "number", + "default": "0.5", + "description": "sampling time for points in predicted path" + }, + "min_velocity_for_map_based_prediction": { + "type": "number", + "default": 1.39, + "description": "apply map-based prediction to the objects with higher velocity than this value" + }, + "min_crosswalk_user_velocity": { + "type": "number", + "default": 1.39, + "description": "minimum velocity use in path prediction for crosswalk users" + }, + "max_crosswalk_user_delta_yaw_threshold_for_lanelet": { + "type": "number", + "default": 0.785, + "description": "maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users" + }, + "dist_threshold_for_searching_lanelet": { + "type": "number", + "default": 3.0, + "description": "The threshold of the angle used when searching for the lane to which the object belongs " + }, + "delta_yaw_threshold_for_searching_lanelet": { + "type": "number", + "default": 0.785, + "description": "The threshold of the distance used when searching for the lane to which the object belongs" + }, + "sigma_lateral_offset": { + "type": "number", + "default": 0.5, + "description": "Standard deviation for lateral position of objects " + }, + "sigma_yaw_angle_deg": { + "type": "number", + "default": 5.0, + "description": "Standard deviation yaw angle of objects " + }, + "object_buffer_time_length": { + "type": "number", + "default": 2.0, + "description": "Time span of object history to store the information" + }, + "history_time_length": { + "type": "number", + "default": 1.0, + "description": "Time span of object information used for prediction" + }, + "prediction_time_horizon_rate_for_validate_shoulder_lane_length": { + "type": "number", + "default": 0.8, + "description": "prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length" + }, + "lane_change_detection": { + "type": "object", + "properties": { + "time_to_change_lane": { + "type": "object", + "properties": { + "dist_threshold_for_lane_change_detection": { + "type": "number", + "default": 1.0 + }, + "time_threshold_for_lane_change_detection": { + "type": "number", + "default": 5.0 + }, + "cutoff_freq_of_velocity_for_lane_change_detection": { + "type": "number", + "default": 0.1 + } + }, + "required": [ + "dist_threshold_for_lane_change_detection", + "time_threshold_for_lane_change_detection", + "cutoff_freq_of_velocity_for_lane_change_detection" + ] + }, + "lat_diff_distance": { + "type": "object", + "properties": { + "dist_ratio_threshold_to_left_bound": { + "type": "number", + "default": -0.5 + }, + "dist_ratio_threshold_to_right_bound": { + "type": "number", + "default": 0.5 + }, + "diff_dist_threshold_to_left_bound": { + "type": "number", + "default": 0.29 + }, + "diff_dist_threshold_to_right_bound": { + "type": "number", + "default": -0.29 + } + }, + "required": [ + "dist_ratio_threshold_to_left_bound", + "dist_ratio_threshold_to_right_bound", + "diff_dist_threshold_to_left_bound", + "diff_dist_threshold_to_right_bound" + ] + } + } + }, + "reference_path_resolution": { + "type": "number", + "default": 0.5, + "description": "Standard deviation for lateral position of objects " + } + }, + "required": [ + "enable_delay_compensation", + "prediction_time_horizon", + "lateral_control_time_horizon", + "prediction_sampling_delta_time", + "min_velocity_for_map_based_prediction", + "min_crosswalk_user_velocity", + "max_crosswalk_user_delta_yaw_threshold_for_lanelet", + "dist_threshold_for_searching_lanelet", + "delta_yaw_threshold_for_searching_lanelet", + "sigma_lateral_offset", + "sigma_yaw_angle_deg", + "object_buffer_time_length", + "history_time_length", + "prediction_time_horizon_rate_for_validate_shoulder_lane_length" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_based_prediction" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} 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 f89c83e9c2d5a..6a6f24081655e 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -415,17 +415,21 @@ bool withinRoadLanelet( } boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const CrosswalkEdgePoints & edge_points, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, - const double min_object_vel) + const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk, + const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); + if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { + return {}; + } + const auto & p1 = edge_points.front_center_point; const auto & p2 = edge_points.back_center_point; @@ -442,62 +446,66 @@ boost::optional isReachableCrosswalkEdgePoints( const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet const auto surrounding_lanelets = lanelet::geometry::findNearest( - lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity); + lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity); - bool first_intersect_load = false; - bool second_intersect_load = false; - std::vector intersects_first; - std::vector intersects_second; - for (const auto & lanelet : surrounding_lanelets) { - if (withinLanelet(object, lanelet.second)) { - return {}; - } + const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) { + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if ( + (attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) && + boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); - if (attr.value() != "road") { - continue; - } + const auto isExist = [](const Point & p, const std::vector & points) { + for (const auto & existingPoint : points) { + if (boost::geometry::distance(p, existingPoint) < 1e-1) { + return true; + } + } + return false; + }; - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}}; - std::vector tmp_intersects; - boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); - for (const auto & p : tmp_intersects) { - intersects_first.push_back(p); + std::vector points_of_intersect; + const boost::geometry::model::linestring line{p_src, p_dst}; + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != lanelet::AttributeValueString::Road) { + continue; } - } - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); for (const auto & p : tmp_intersects) { - intersects_second.push_back(p); + if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { + continue; + } + points_of_intersect.push_back(p); + if (points_of_intersect.size() >= 2) { + return true; + } } } - } + return false; + }; - if (1 < intersects_first.size()) { - first_intersect_load = true; - } + const bool first_intersects_road = isAcrossAnyRoad( + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); + const bool second_intersects_road = + isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); - if (1 < intersects_second.size()) { - second_intersect_load = true; - } - - if (first_intersect_load && second_intersect_load) { + if (first_intersects_road && second_intersects_road) { return {}; } - if (first_intersect_load && !second_intersect_load) { + if (first_intersects_road && !second_intersects_road) { ret.swap(); } @@ -605,16 +613,16 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw // constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; + + constexpr float high_speed_threshold = tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); const bool high_speed_object = abs_speed > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled if (high_speed_object) return label; // Do nothing for now @@ -740,6 +748,12 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg"); object_buffer_time_length_ = declare_parameter("object_buffer_time_length"); history_time_length_ = declare_parameter("history_time_length"); + + check_lateral_acceleration_constraints_ = + declare_parameter("check_lateral_acceleration_constraints"); + max_lateral_accel_ = declare_parameter("max_lateral_accel"); + min_acceleration_before_curve_ = declare_parameter("min_acceleration_before_curve"); + { // lane change detection lane_change_detection_method_ = declare_parameter("lane_change_detection.method"); @@ -789,6 +803,25 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "max_lateral_accel", max_lateral_accel_); + updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); + updateParam( + parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; } PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( @@ -972,16 +1005,53 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Generate Predicted Path std::vector predicted_paths; + double min_avg_curvature = std::numeric_limits::max(); + PredictedPath path_with_smallest_avg_curvature; + for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( yaw_fixed_transformed_object, ref_path.path); - if (predicted_path.path.empty()) { + if (predicted_path.path.empty()) continue; + + if (!check_lateral_acceleration_constraints_) { + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); + continue; + } + + // Check lat. acceleration constraints + const auto trajectory_with_const_velocity = + toTrajectoryPoints(predicted_path, abs_obj_speed); + + if (isLateralAccelerationConstraintSatisfied( + trajectory_with_const_velocity, prediction_sampling_time_interval_)) { + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); continue; } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); + + // Calculate curvature assuming the trajectory points interval is constant + // In case all paths are deleted, a copy of the straightest path is kept + + constexpr double curvature_calculation_distance = 2.0; + constexpr double points_interval = 1.0; + const size_t idx_dist = static_cast( + std::max(static_cast((curvature_calculation_distance) / points_interval), 1)); + const auto curvature_v = + calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist); + if (curvature_v.empty()) { + continue; + } + const auto curvature_avg = + std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size(); + if (curvature_avg < min_avg_curvature) { + min_avg_curvature = curvature_avg; + path_with_smallest_avg_curvature = predicted_path; + path_with_smallest_avg_curvature.confidence = ref_path.probability; + } } + if (predicted_paths.empty()) predicted_paths.push_back(path_with_smallest_avg_curvature); // Normalize Path Confidence and output the predicted object float sum_confidence = 0.0; @@ -1135,48 +1205,45 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } + } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge + for (const auto & crosswalk : crosswalks_) { + const auto edge_points = getCrosswalkEdgePoints(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + const auto reachable_second = hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + + if (!reachable_first && !reachable_second) { + continue; + } - // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge - // points for all crosswalks and generate path to the crosswalk edge - } else { - for (const auto & crosswalk : crosswalks_) { - const auto edge_points = getCrosswalkEdgePoints(crosswalk); - - const auto reachable_first = hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - const auto reachable_second = hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - - if (!reachable_first && !reachable_second) { - continue; - } - - const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, edge_points, lanelet_map_ptr_, prediction_time_horizon_, - min_crosswalk_user_velocity_); - - if (!reachable_crosswalk) { - continue; - } + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + min_crosswalk_user_velocity_); - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); - predicted_path.confidence = 1.0; + if (!reachable_crosswalk) { + continue; + } - if (predicted_path.path.empty()) { - continue; - } - // If the predicted path to the crosswalk is crossing the fence, don't use it - if (doesPathCrossAnyFence(predicted_path)) { - continue; - } + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + if (predicted_path.path.empty()) { + continue; + } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; } + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); } const auto n_path = predicted_object.kinematics.predicted_paths.size(); diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 5cb7e186b7cc4..d3f610583577c 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -162,7 +162,7 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const double ep = 0.001; + constexpr double ep = 0.001; const double duration = time_horizon_ + ep; PredictedPath path; diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 3e379bcfd1cf1..f5fbc8ff950e8 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() ### Find Eigen Dependencies find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(glog REQUIRED) include_directories( SYSTEM @@ -40,6 +41,7 @@ ament_auto_add_library(multi_object_tracker_node SHARED target_link_libraries(multi_object_tracker_node Eigen3::Eigen + glog::glog ) rclcpp_components_register_node(multi_object_tracker_node diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index 3d30add05392a..ca320c7f58442 100644 --- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -17,5 +17,5 @@ # debug parameters publish_processing_time: false publish_tentative_objects: false - diagnostic_warn_delay: 0.5 # [sec] - diagnostic_error_delay: 1.0 # [sec] + diagnostics_warn_delay: 0.5 # [sec] + diagnostics_error_delay: 1.0 # [sec] diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index e3172dfd22349..763bd12fab79e 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -13,8 +13,10 @@ eigen3_cmake_module autoware_auto_perception_msgs + diagnostic_updater eigen kalman_filter + libgoogle-glog-dev mussp object_recognition_utils rclcpp diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d28833241bd5f..1d4d50c7eab4c 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -16,6 +16,7 @@ #include +#include #include #include @@ -177,6 +178,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("multi_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp b/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp deleted file mode 100644 index 8432a4e870e20..0000000000000 --- a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright 2021 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ - -#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" -#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp" -#include "object_association_merger/data_association/solver/successive_shortest_path.hpp" - -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/data_association.hpp b/perception/object_merger/include/object_merger/data_association/data_association.hpp similarity index 84% rename from perception/object_merger/include/object_association_merger/data_association/data_association.hpp rename to perception/object_merger/include/object_merger/data_association/data_association.hpp index c03dd40b0d3b7..4cd9302fb29db 100644 --- a/perception/object_merger/include/object_association_merger/data_association/data_association.hpp +++ b/perception/object_merger/include/object_merger/data_association/data_association.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #include #include @@ -25,7 +25,7 @@ #include #define EIGEN_MPL2_ONLY -#include "object_association_merger/data_association/solver/gnn_solver.hpp" +#include "object_merger/data_association/solver/gnn_solver.hpp" #include #include @@ -56,4 +56,4 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..cead11d516421 --- /dev/null +++ b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// Copyright 2021 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 OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "object_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp similarity index 79% rename from perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp rename to perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp index ec4bc907d85f6..447f4fa2b7ce3 100644 --- a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp +++ b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace gnn_solver -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp similarity index 72% rename from perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp rename to perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp index f9d88bd793db1..0464a29e6b2e5 100644 --- a/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_merger/data_association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace gnn_solver -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp b/perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp similarity index 72% rename from perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp rename to perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp index 2487faaa07c0e..618879a01866f 100644 --- a/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp +++ b/perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_merger/data_association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace gnn_solver -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp similarity index 93% rename from perception/object_merger/include/object_association_merger/node.hpp rename to perception/object_merger/include/object_merger/node.hpp index 6815b59894083..8341ce490ca72 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__NODE_HPP_ -#define OBJECT_ASSOCIATION_MERGER__NODE_HPP_ +#ifndef OBJECT_MERGER__NODE_HPP_ +#define OBJECT_MERGER__NODE_HPP_ -#include "object_association_merger/data_association/data_association.hpp" +#include "object_merger/data_association/data_association.hpp" #include @@ -84,4 +84,4 @@ class ObjectAssociationMergerNode : public rclcpp::Node }; } // namespace object_association -#endif // OBJECT_ASSOCIATION_MERGER__NODE_HPP_ +#endif // OBJECT_MERGER__NODE_HPP_ diff --git a/perception/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_merger/utils/utils.hpp similarity index 90% rename from perception/object_merger/include/object_association_merger/utils/utils.hpp rename to perception/object_merger/include/object_merger/utils/utils.hpp index bb4466bd4944d..40cade6cbbccb 100644 --- a/perception/object_merger/include/object_association_merger/utils/utils.hpp +++ b/perception/object_merger/include/object_merger/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ -#define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ +#ifndef OBJECT_MERGER__UTILS__UTILS_HPP_ +#define OBJECT_MERGER__UTILS__UTILS_HPP_ #include #include @@ -70,4 +70,4 @@ enum MSG_COV_IDX { }; } // namespace utils -#endif // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ +#endif // OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp index 4eefcc1702c3b..4ccecbb9f60dd 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/data_association/data_association.hpp" +#include "object_merger/data_association/data_association.hpp" -#include "object_association_merger/data_association/solver/gnn_solver.hpp" -#include "object_association_merger/utils/utils.hpp" +#include "object_merger/data_association/solver/gnn_solver.hpp" +#include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp index bb3d1ed8fdaa4..e7fa56fee88c5 100644 --- a/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp" #include diff --git a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp index 904d57b71f072..b38257d0188e0 100644 --- a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/data_association/solver/successive_shortest_path.hpp" +#include "object_merger/data_association/solver/successive_shortest_path.hpp" #include #include diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index f776d2d940045..1ffc2791e8f4e 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/node.hpp" +#include "object_merger/node.hpp" -#include "object_association_merger/utils/utils.hpp" +#include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 035845772ce53..928532f928e38 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -55,6 +55,24 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map EXECUTABLE laserscan_based_occupancy_grid_map_node ) +# GridMapFusionNode +ament_auto_add_library(synchronized_grid_map_fusion SHARED + src/fusion/synchronized_grid_map_fusion_node.cpp + src/fusion/single_frame_fusion_policy.cpp + src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp + src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp + src/utils/utils.cpp +) + +target_link_libraries(synchronized_grid_map_fusion + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(synchronized_grid_map_fusion + PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + EXECUTABLE synchronized_grid_map_fusion_node +) + ament_auto_package( INSTALL_TO_SHARE launch @@ -66,13 +84,22 @@ if(BUILD_TESTING) # launch_testing find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/test_pointcloud_based_method.py) + add_launch_test(test/test_synchronized_grid_map_fusion_node.py) # gtest ament_add_gtest(test_utils test/test_utils.cpp ) + ament_add_gtest(costmap_unit_tests + test/cost_value_test.cpp) + ament_add_gtest(fusion_policy_unit_tests + test/fusion_policy_test.cpp + src/fusion/single_frame_fusion_policy.cpp + ) target_link_libraries(test_utils ${PCL_LIBRARIES} ${PROJECT_NAME}_common ) + target_include_directories(costmap_unit_tests PRIVATE "include") + target_include_directories(fusion_policy_unit_tests PRIVATE "include") endif() diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 962bcdc6f154f..084c55d80d629 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -9,6 +9,7 @@ This package outputs the probability of having an obstacle as occupancy grid map - [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) - [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) +- [Grid map fusion](synchronized_grid_map_fusion.md) ## Settings @@ -70,3 +71,19 @@ Additional argument is shown below: | `container_name` | `occupancy_grid_map_container` | | | `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | | `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | + +### Test + +This package provides unit tests using `gtest`. +You can run the test by the following command. + +```bash +colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+ +``` + +Test contains the following. + +- Unit test for cost value conversion function +- Unit test for utility functions +- Unit test for occupancy grid map fusion functions +- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml new file mode 100644 index 0000000000000..f8a2dc2fbc7de --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # 1. fusion parameters + fusion_input_ogm_topics: ["topic1", "topic2"] + input_ogm_reliabilities: [0.8, 0.2] + fusion_method: "overwrite" # choose from ["overwrite", "log-odds", "dempster-shafer"] + + # 2. synchronization settings + match_threshold_sec: 0.01 # 10ms + timeout_sec: 0.1 # 100ms + input_offset_sec: [0.0, 0.0] # no offset + + # 3. settings for fused fusion map + # remember resolution and map size should be same with input maps + map_frame_: "map" + base_link_frame_: "base_link" + grid_map_origin_frame_: "base_link" + fusion_map_length_x: 100.0 + fusion_map_length_y: 100.0 + fusion_map_resolution: 0.5 diff --git a/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg new file mode 100644 index 0000000000000..6bf5ed98e4edf --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg @@ -0,0 +1,290 @@ + + + + + + + + + + +
+
+
+ OGM1 +
+
+
+ + OGM1 + + + + + + + + +
+
+
+ OGM2 +
+
+
+ + OGM2 + + + + + + + + +
+
+
+ OGM3 +
+
+
+ + OGM3 + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+ (Single Frame) +
+
+
+ + Fused... + + + + + + + + +
+
+
+ + Single Frame +
+ Fusion + +
+
+
+ + Single Frame... + + + + + + + + +
+
+
+ + Multi Frame +
+ Fusion + +
+
+
+ + Multi Frame... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
`t=t_{n-1}`
+
+
+ + `t=t_{n-1}` + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
+ Publish +
+
+
+ + Publish + + + + + + +
+
+
+ input topics +
+
+
+ + input topics + + + + + + +
+
+
+ output topics +
+
+
+ + output topics + + + + + + +
+
+
+ grid_map_fusion::GridMapFusionNode +
+
+
+ + grid_map_fusion::GridMapFusionNode + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp similarity index 73% rename from perception/probabilistic_occupancy_grid_map/include/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp index e9667ccf65577..880297a1210ed 100644 --- a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp @@ -48,8 +48,8 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef COST_VALUE_HPP_ -#define COST_VALUE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ #include @@ -59,19 +59,48 @@ static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; static const unsigned char FREE_SPACE = 0; +static const unsigned char OCCUPIED_THRESHOLD = 180; +static const unsigned char FREE_THRESHOLD = 50; + struct CostTranslationTable { CostTranslationTable() { for (int i = 0; i < 256; i++) { - const auto value = static_cast(static_cast(i) * 100.f / 255.f); - data[i] = std::max(std::min(value, static_cast(99)), static_cast(1)); + const auto value = + static_cast(static_cast(i) * 100.f / 255.f); // 0-255 to 0-100 + data[i] = + std::max(std::min(value, static_cast(99)), static_cast(1)); // 0-100 to 1-99 } } char operator[](unsigned char n) const { return data[n]; } char data[256]; }; +struct InverseCostTranslationTable +{ + InverseCostTranslationTable() + { + // 0-100 to 0-255 + for (int i = 0; i < 100; i++) { + data[i] = static_cast(i * 255 / 99); + } + } + unsigned char operator[](char n) const + { + if (n > 99) { + return data[99]; + } else if (n < 1) { + return data[1]; + } else { + const unsigned char u_n = static_cast(n); + return data[u_n]; + } + } + unsigned char data[100]; +}; + static const CostTranslationTable cost_translation_table; +static const InverseCostTranslationTable inverse_cost_translation_table; } // namespace occupancy_cost_value -#endif // COST_VALUE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp new file mode 100644 index 0000000000000..1f0553878ef5a --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp @@ -0,0 +1,66 @@ +// 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 PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" + +#include +#include +#include +#include + +/*min and max prob threshold to prevent log-odds to diverge*/ +#define EPSILON_PROB 0.03 + +namespace fusion_policy +{ +enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; + +unsigned char convertProbabilityToChar(const double & probability); +double convertCharToProbability(const unsigned char & occupancy); +std::vector convertProbabilityToChar(const std::vector & probabilities); +std::vector convertCharToProbability(const std::vector & occupancies); + +namespace overwrite_fusion +{ +enum State : unsigned char { UNKNOWN = 0U, FREE = 1U, OCCUPIED = 2U }; +State getApproximateState(const unsigned char & occupancy); +unsigned char overwriteFusion(const std::vector & occupancies); +} // namespace overwrite_fusion + +namespace log_odds_fusion +{ +double logOddsFusion(const std::vector & probabilities); +double logOddsFusion( + const std::vector & probabilities, const std::vector & weights); +} // namespace log_odds_fusion + +namespace dempster_shafer_fusion +{ +struct dempsterShaferOccupancy; +double dempsterShaferFusion(const std::vector & probability); +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability); +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method); +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability); +} // namespace fusion_policy + +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp new file mode 100644 index 0000000000000..8f09764688055 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -0,0 +1,127 @@ +// 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 PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ + +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace synchronized_grid_map_fusion +{ + +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using costmap_2d::OccupancyGridMapUpdaterInterface; +using geometry_msgs::msg::Pose; + +class GridMapFusionNode : public rclcpp::Node +{ +public: + explicit GridMapFusionNode(const rclcpp::NodeOptions & node_options); + +private: + void onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & input_topic); + nav_msgs::msg::OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); + + nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); + + void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg); + + void setPeriod(const int64_t new_period); + void timer_callback(); + void publish(); + +private: + // Publisher and Subscribers + rclcpp::Publisher::SharedPtr fused_map_pub_; + rclcpp::Publisher::SharedPtr single_frame_pub_; + std::vector::SharedPtr> grid_map_subs_; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; + + // Topics manager + std::size_t num_input_topics_{1}; + std::vector input_topics_; + std::vector input_offset_sec_; + std::vector input_topic_weights_; + std::map input_topic_weights_map_; + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Timer to manage the timeout of the occupancy grid map + rclcpp::TimerBase::SharedPtr timer_; + double timeout_sec_{}; + double match_threshold_sec_{}; + + // cache for fusion + std::mutex mutex_; + std::shared_ptr + occupancy_grid_map_updater_ptr_; // contains fused grid map + std::map + gridmap_dict_; // temporary cache for grid map message + std::map + gridmap_dict_tmp_; // second cache for grid map message + std::map offset_map_; // time offset for each grid map + + // grid map parameters + std::string map_frame_; + std::string base_link_frame_; + std::string gridmap_origin_frame_; + float fusion_map_length_x_; + float fusion_map_length_y_; + float fusion_map_resolution_; + fusion_policy::FusionMethod fusion_method_; +}; + +} // namespace synchronized_grid_map_fusion + +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index 3a0f1f5ea7433..a8adea6e700e5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -102,4 +102,4 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp index a9bbc3fda6ab4..d2210cf9c348a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -91,4 +91,4 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index 8f8e1e503d388..f01b2d74e160b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -92,4 +92,4 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index 5755cd2c889be..298f327d632d7 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" namespace costmap_2d { @@ -44,4 +44,4 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp similarity index 78% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index bc278772e9737..67b51d6184c8c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" #include @@ -52,4 +52,4 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 83% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index e34899bb98b0c..93486e0ca56af 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -95,4 +95,4 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp index d8f3cb6556bf2..3a921035ef219 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -39,4 +39,4 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp new file mode 100644 index 0000000000000..f9f100f285d38 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ + +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" + +#include +#include + +// LOBF means: Log Odds Bayes Filter + +namespace costmap_2d +{ +class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface +{ +public: + enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + OccupancyGridMapLOBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + { + } + bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + void initRosParam(rclcpp::Node & node) override; + +private: + inline unsigned char applyLOBF(const unsigned char & z, const unsigned char & o); + Eigen::Matrix2f probability_matrix_; +}; + +} // namespace costmap_2d + +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp similarity index 78% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp index 9305313ddc2fc..e85edf2245ef3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -38,4 +38,4 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp index 047b747c2861f..0950272dac470 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -114,6 +116,7 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); +unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils -#endif // UTILS__UTILS_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml new file mode 100644 index 0000000000000..c2391141e9fa0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp new file mode 100644 index 0000000000000..6ac681eff4916 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -0,0 +1,322 @@ +// 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 "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" + +namespace fusion_policy +{ + +//// utils //// + +// convert [0, 1] to [0, 255] +unsigned char convertProbabilityToChar(const double & probability) +{ + return static_cast(probability * 255); +} +// convert [0, 255] to [0, 1] +double convertCharToProbability(const unsigned char & occupancy) +{ + return static_cast(occupancy) / 255.0; +} + +// convert [0, 1] to [0, 255] vectors +std::vector convertProbabilityToChar(const std::vector & probabilities) +{ + std::vector occupancies; + for (auto & probability : probabilities) { + occupancies.push_back(convertProbabilityToChar(probability)); + } + return occupancies; +} +// convert [0, 255] to [0, 1] vectors +std::vector convertCharToProbability(const std::vector & occupancies) +{ + std::vector probabilities; + for (auto & occupancy : occupancies) { + probabilities.push_back(convertCharToProbability(occupancy)); + } + return probabilities; +} + +/// @brief fusion with overwrite policy +namespace overwrite_fusion +{ + +/** + * @brief convert char value to occupancy state + * + * @param occupancy [0, 255] + * @return State + */ +State getApproximateState(const unsigned char & occupancy) +{ + if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return State::OCCUPIED; + } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + return State::FREE; + } else { + return State::UNKNOWN; + } +} + +/** + * @brief override fusion + * + * @param occupancies : occupancies to be fused + * @return unsigned char + */ +unsigned char overwriteFusion(const std::vector & occupancies) +{ + if (occupancies.size() == 0) { + throw std::runtime_error("occupancies size is 0"); + } else if (occupancies.size() == 1) { + return occupancies[0]; + } + + auto fusion_occupancy = 128; // unknown + auto fusion_state = getApproximateState(fusion_occupancy); + for (auto & cell : occupancies) { + auto state = getApproximateState(cell); + if (state > fusion_state) { + fusion_state = state; + fusion_occupancy = cell; + } else if (state < fusion_state) { + continue; + } else { + fusion_occupancy = (fusion_occupancy / 2 + cell / 2); + } + } + return fusion_occupancy; +} +} // namespace overwrite_fusion + +/// @brief fusion with log-odds policy +namespace log_odds_fusion +{ +/** + * @brief log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @return double + */ +double logOddsFusion(const std::vector & probabilities) +{ + double log_odds = 0.0; + for (auto & probability : probabilities) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probability)); + log_odds += std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} + +/** + * @brief weighted log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @param weights : weights of probabilities + * @return double + */ +double logOddsFusion(const std::vector & probabilities, const std::vector & weights) +{ + // check if the size of probabilities and weights are the same + if (probabilities.size() != weights.size()) { + // warning and return normal log-odds fusion + std::cout + << "The size of probabilities and weights are not the same. Return normal log-odds fusion." + << std::endl; + return logOddsFusion(probabilities); + } + + double log_odds = 0.0; + for (size_t i = 0; i < probabilities.size(); i++) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probabilities[i])); + log_odds += weights[i] * std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} +} // namespace log_odds_fusion + +/// @brief fusion with Dempster-Shafer Theory +namespace dempster_shafer_fusion +{ +/** + * @brief conflict modified Dempster-Shafer Theory Data structure + * see https://www.diva-portal.org/smash/get/diva2:852457/FULLTEXT01.pdf + * + */ +struct dempsterShaferOccupancy +{ + double occupied; + double empty; + double unknown; + double conflict_threshold = 0.9; + + // initialize without args + dempsterShaferOccupancy() + { + occupied = 0.0; + empty = 0.0; + unknown = 1.0; + } + + // initialize with probability + dempsterShaferOccupancy(double occupied_probability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p; + empty = 1.0 - p; + unknown = 0.0; + } + + // initialize with probability and reliability + dempsterShaferOccupancy(double occupied_probability, double reliability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p * reliability; + empty = (1.0 - p) * reliability; + unknown = 1.0 - occupied - empty; + } + + // normalize + void normalize() + { + double sum = occupied + empty + unknown; + occupied /= sum; + empty /= sum; + unknown /= sum; + } + + // calc conflict factor K + double calcK(const dempsterShaferOccupancy & other) + { + return (occupied * other.empty + empty * other.occupied); + } + // calc sum of occupied probability mass + double calcOccupied(const dempsterShaferOccupancy & other) + { + return occupied * other.occupied + occupied * other.unknown + unknown * other.occupied; + } + // calc sum of empty probability mass + double calcEmpty(const dempsterShaferOccupancy & other) + { + return empty * other.empty + empty * other.unknown + unknown * other.empty; + } + + // Dempster-Shafer fusion + dempsterShaferOccupancy operator+(const dempsterShaferOccupancy & other) + { + dempsterShaferOccupancy result; + double K = calcK(other); + double O = calcOccupied(other); + double E = calcEmpty(other); + + if (K > conflict_threshold) { + // highly conflict + result.occupied = O; + result.empty = E; + result.unknown = 1 - O - E; + } else { + // low conflict + result.occupied = O / (1.0 - K); + result.empty = E / (1.0 - K); + result.unknown = 1 - result.occupied - result.empty; + } + return result; + } + + // get occupancy probability via Pignistic Probability + double getPignisticProbability() { return occupied + unknown / 2.0; } +}; + +/** + * @brief Dempster-Shafer fusion + * + * @param probability + * @return double + */ +double dempsterShaferFusion(const std::vector & probability) +{ + dempsterShaferOccupancy result; // init with unknown + for (auto & p : probability) { + result = result + dempsterShaferOccupancy(p); + } + return result.getPignisticProbability(); +} + +/** + * @brief Dempster-Shafer fusion with reliability + * + * @param probability + * @param reliability + * @return double + */ +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability) +{ + // check if the size of probabilities and weights are the same + if (probability.size() != reliability.size()) { + // warning and return normal dempster-shafer fusion probability + std::cout << "The size of probabilities and reliability are not the same. Return normal " + "dempster-shafer fusion." + << std::endl; + return dempsterShaferFusion(probability); + } + + dempsterShaferOccupancy result; // init with unknown + for (size_t i = 0; i < probability.size(); i++) { + result = result + dempsterShaferOccupancy(probability[i], reliability[i]); + } + return result.getPignisticProbability(); +} +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(dempster_shafer_fusion::dempsterShaferFusion(probability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability, reliability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar( + dempster_shafer_fusion::dempsterShaferFusion(probability, reliability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +} // namespace fusion_policy diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp new file mode 100644 index 0000000000000..a88e65e712ac1 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -0,0 +1,458 @@ +// 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 "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" + +namespace synchronized_grid_map_fusion +{ +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; + +GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) +: Node("synchronized_occupancy_grid_map_fusion", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + /* load input parameters */ + { + // get input topics + declare_parameter("fusion_input_ogm_topics", std::vector{}); + input_topics_ = get_parameter("fusion_input_ogm_topics").as_string_array(); + if (input_topics_.empty()) { + throw std::runtime_error("The number of input topics must larger than 0."); + } + num_input_topics_ = input_topics_.size(); + if (num_input_topics_ < 1) { + RCLCPP_WARN( + this->get_logger(), "minimum num_input_topics_ is 1. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 1; + } + if (num_input_topics_ > 12) { + RCLCPP_WARN( + this->get_logger(), "maximum num_input_topics_ is 12. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 12; + } + // get input topic reliabilities + declare_parameter("input_ogm_reliabilities", std::vector{}); + input_topic_weights_ = get_parameter("input_ogm_reliabilities").as_double_array(); + + if (input_topic_weights_.empty()) { // no topic weight is set + // set equal weight + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = 1.0 / num_input_topics_; + } + } else if (num_input_topics_ == input_topic_weights_.size()) { + // set weight to map + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = input_topic_weights_.at(topic_i); + } + } else { + throw std::runtime_error("The number of weights does not match the number of topics."); + } + } + + // Set fusion timing parameters + match_threshold_sec_ = declare_parameter("match_threshold_sec", 0.01); + timeout_sec_ = declare_parameter("timeout_sec", 0.1); + input_offset_sec_ = declare_parameter("input_offset_sec", std::vector{}); + if (!input_offset_sec_.empty() && num_input_topics_ != input_offset_sec_.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } else if (input_offset_sec_.empty()) { + // if there are not input offset, set 0.0 + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_offset_sec_.push_back(0.0); + } + } + // Set fusion map parameters + map_frame_ = declare_parameter("map_frame_", "map"); + base_link_frame_ = declare_parameter("base_link_frame_", "base_link"); + gridmap_origin_frame_ = declare_parameter("grid_map_origin_frame_", "base_link"); + fusion_map_length_x_ = declare_parameter("fusion_map_length_x", 100.0); + fusion_map_length_y_ = declare_parameter("fusion_map_length_y", 100.0); + fusion_map_resolution_ = declare_parameter("fusion_map_resolution", 0.5); + + // set fusion method + std::string fusion_method_str = declare_parameter("fusion_method", "overwrite"); + if (fusion_method_str == "overwrite") { + fusion_method_ = fusion_policy::FusionMethod::OVERWRITE; + } else if (fusion_method_str == "log-odds") { + fusion_method_ = fusion_policy::FusionMethod::LOG_ODDS; + } else if (fusion_method_str == "dempster-shafer") { + fusion_method_ = fusion_policy::FusionMethod::DEMPSTER_SHAFER; + } else { + throw std::runtime_error("The fusion method is not supported."); + } + + // sub grid_map + grid_map_subs_.resize(num_input_topics_); + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + std::function fnc = std::bind( + &GridMapFusionNode::onGridMap, this, std::placeholders::_1, input_topics_.at(topic_i)); + grid_map_subs_.at(topic_i) = this->create_subscription( + input_topics_.at(topic_i), rclcpp::QoS{1}.best_effort(), fnc); + } + + // pub grid_map + fused_map_pub_ = this->create_publisher( + "~/output/occupancy_grid_map", rclcpp::QoS{1}.reliable()); + single_frame_pub_ = this->create_publisher( + "~/debug/single_frame_map", rclcpp::QoS{1}.reliable()); + + // updater + occupancy_grid_map_updater_ptr_ = std::make_shared( + fusion_map_length_x_ / fusion_map_resolution_, fusion_map_length_y_ / fusion_map_resolution_, + fusion_map_resolution_); + + // Set timer + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&GridMapFusionNode::timer_callback, this)); + + // set offset map + for (size_t i = 0; i < input_offset_sec_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_sec_[i]; + } + + // init dict + for (size_t i = 0; i < input_topics_.size(); ++i) { + gridmap_dict_[input_topics_[i]] = nullptr; + gridmap_dict_tmp_[input_topics_[i]] = nullptr; + } + + // debug tools + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } +} + +/** + * @brief Set the period of the timer + * + * @param new_period + */ +void GridMapFusionNode::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +/** + * @brief Callback function for the grid map message add data to buffer + * + * @param occupancy_grid_msg + * @param topic_name + */ +void GridMapFusionNode::onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + const bool is_already_subscribed_this = (gridmap_dict_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + // already subscribed + if (is_already_subscribed_this) { + gridmap_dict_tmp_[topic_name] = occupancy_grid_msg; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + // first time to subscribe in this period + } else { + gridmap_dict_[topic_name] = occupancy_grid_msg; // add to buffer + + // check if all topics are subscribed + bool is_subscribed_all = std::all_of( + std::begin(gridmap_dict_), std::end(gridmap_dict_), + [](const auto & e) { return e.second != nullptr; }); + // Then, go to publish + if (is_subscribed_all) { + for (const auto & e : gridmap_dict_tmp_) { + if (e.second != nullptr) { + gridmap_dict_[e.first] = e.second; + } + } + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + // if not all topics are subscribed, set timer + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +/** + * @brief Timer callback function + * + */ +void GridMapFusionNode::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +/** + * @brief Publisher function + * + */ +void GridMapFusionNode::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + builtin_interfaces::msg::Time latest_stamp = get_clock()->now(); + double height = 0.0; + + // merge available gridmap + std::vector subscribed_maps; + std::vector weights; + for (const auto & e : gridmap_dict_) { + if (e.second != nullptr) { + subscribed_maps.push_back(GridMapFusionNode::OccupancyGridMsgToGridMap(*e.second)); + weights.push_back(input_topic_weights_map_[e.first]); + latest_stamp = e.second->header.stamp; + height = e.second->info.origin.position.z; + } + } + + // return if empty + if (subscribed_maps.empty()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "No grid map is subscribed. Please check the topic name and the topic type."); + return; + } + + // fusion map + // single frame gridmap fusion + auto fused_map = SingleFrameOccupancyFusion(subscribed_maps, latest_stamp, weights); + // multi frame gridmap fusion + occupancy_grid_map_updater_ptr_->update(fused_map); + + // publish + fused_map_pub_->publish( + OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, *occupancy_grid_map_updater_ptr_)); + single_frame_pub_->publish(OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, fused_map)); + + // copy 2nd temp to temp buffer + gridmap_dict_ = gridmap_dict_tmp_; + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights) +{ + // if only single map + if (occupancy_grid_maps.size() == 1) { + return occupancy_grid_maps[0]; + } + + // get map to gridmap_origin_frame_ transform + Pose gridmap_origin{}; + try { + gridmap_origin = utils::getPose(latest_stamp, tf_buffer_, gridmap_origin_frame_, map_frame_); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(get_logger(), ex.what()); + return occupancy_grid_maps[0]; + } + + // init fused map with calculated origin + OccupancyGridMapFixedBlindSpot fused_map( + occupancy_grid_maps[0].getSizeInCellsX(), occupancy_grid_maps[0].getSizeInCellsY(), + occupancy_grid_maps[0].getResolution()); + fused_map.updateOrigin( + gridmap_origin.position.x - fused_map.getSizeInMetersX() / 2, + gridmap_origin.position.y - fused_map.getSizeInMetersY() / 2); + + // fix origin of each map + for (auto & map : occupancy_grid_maps) { + map.updateOrigin(fused_map.getOriginX(), fused_map.getOriginY()); + } + + // assume map is same size and resolutions + for (unsigned int x = 0; x < fused_map.getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < fused_map.getSizeInCellsY(); y++) { + // get cost of each map + std::vector costs; + for (auto & map : occupancy_grid_maps) { + costs.push_back(map.getCost(x, y)); + } + + // set fusion policy + auto fused_cost = fusion_policy::singleFrameOccupancyFusion(costs, fusion_method_, weights); + + // set max cost to fused map + fused_map.setCost(x, y, fused_cost); + } + } + + return fused_map; +} + +/** + * @brief Update occupancy grid map with asynchronous input (currently unused) + * + * @param occupancy_grid_msg + */ +void GridMapFusionNode::updateGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg) +{ + // get updater map origin + + // origin is set to current updater map + auto map_for_update = OccupancyGridMsgToCostmap2D(*occupancy_grid_msg); + + // update + occupancy_grid_map_updater_ptr_->update(map_for_update); +} + +nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + nav2_costmap_2d::Costmap2D costmap2d( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution, occupancy_grid_map.info.origin.position.x, + occupancy_grid_map.info.origin.position.y, 0); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + costmap2d.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + + return costmap2d; +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + OccupancyGridMapFixedBlindSpot gridmap( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution); + gridmap.updateOrigin( + occupancy_grid_map.info.origin.position.x, occupancy_grid_map.info.origin.position.y); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + gridmap.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + return gridmap; +} + +nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map) +{ + auto msg_ptr = std::make_unique(); + + msg_ptr->header.frame_id = frame_id; + msg_ptr->header.stamp = stamp; + msg_ptr->info.resolution = occupancy_grid_map.getResolution(); + + msg_ptr->info.width = occupancy_grid_map.getSizeInCellsX(); + msg_ptr->info.height = occupancy_grid_map.getSizeInCellsY(); + + double wx{}; + double wy{}; + occupancy_grid_map.mapToWorld(0, 0, wx, wy); + msg_ptr->info.origin.position.x = occupancy_grid_map.getOriginX(); + msg_ptr->info.origin.position.y = occupancy_grid_map.getOriginY(); + msg_ptr->info.origin.position.z = robot_pose_z; + msg_ptr->info.origin.orientation.w = 1.0; + + msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); + + unsigned char * data = occupancy_grid_map.getCharMap(); + for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { + msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + } + return msg_ptr; +} + +} // namespace synchronized_grid_map_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b4505eedddd21..777c180fe1a3a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp index 59f562cb58f35..3d02be9ca7156 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -49,9 +49,9 @@ * David V. Lu!! *********************************************************************/ -#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 8652cfa34d96c..3d176e41583a0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 56aeea30e0773..00b3e42fdf392 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 20a5770e37fdb..64b76a2488e24 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index f6369602b8890..82da92da7f146 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" -#include "cost_value.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp index 16de17b1c2e61..40f538a64f6e9 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp new file mode 100644 index 0000000000000..f3e306f262bf0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -0,0 +1,69 @@ +// 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 "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" + +#include + +namespace costmap_2d +{ + +void OccupancyGridMapLOBFUpdater::initRosParam(rclcpp::Node & /*node*/) +{ + // nothing to load +} + +inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( + const unsigned char & z, const unsigned char & o) +{ + using fusion_policy::convertCharToProbability; + using fusion_policy::convertProbabilityToChar; + using fusion_policy::log_odds_fusion::logOddsFusion; + + constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown_margin = 1; + /* Tau and ST decides how fast the observation decay to the unknown status*/ + constexpr double tau = 0.75; + constexpr double sample_time = 0.1; + + // if the observation is unknown, decay the estimation + if (z >= unknown - unknown_margin && z <= unknown + unknown_margin) { + char diff = static_cast(o) - static_cast(unknown); + const double decay = std::exp(-sample_time / tau); + const double fused = static_cast(unknown) + static_cast(diff) * decay; + return static_cast(fused); + } else { + // else, do the log-odds fusion + const std::vector probability = { + convertCharToProbability(z), convertCharToProbability(o)}; + const unsigned char fused = convertProbabilityToChar(logOddsFusion(probability)); + return fused; + } +} + +bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +{ + updateOrigin( + single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyLOBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } + } + return true; +} +} // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 0060754cd875c..8009a503167ea 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -182,4 +182,20 @@ bool extractCommonPointCloud( return true; } +/** + * @brief Convert unsigned char value to closest cost value + * @param cost Cost value + * @return Probability + */ +unsigned char getApproximateOccupancyState(const unsigned char & value) +{ + if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return occupancy_cost_value::LETHAL_OBSTACLE; + } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { + return occupancy_cost_value::FREE_SPACE; + } else { + return occupancy_cost_value::NO_INFORMATION; + } +} + } // namespace utils diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md new file mode 100644 index 0000000000000..e1046fa24719d --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -0,0 +1,168 @@ +# synchronized OGM fusion + +> For simplicity, we use OGM as the meaning of the occupancy grid map. + +This package is used to fuse the OGMs from synchronized sensors. Especially for the lidar. + +Here shows the example OGM for the this synchronized OGM fusion. + +| left lidar OGM | right lidar OGM | top lidar OGM | +| --------------------------------- | ----------------------------------- | ------------------------------- | +| ![left](image/left_lidar_ogm.png) | ![right](image/right_lidar_ogm.png) | ![top](image/top_lidar_ogm.png) | + +OGM fusion with asynchronous sensor outputs is not suitable for this package. Asynchronous OGM fusion is under construction. + +## Processing flow + +The processing flow of this package is shown in the following figure. + +![data_flow](image/synchronized_grid_map_fusion.drawio.svg) + +- Single Frame Fusion + - Single frame fusion means that the OGMs from synchronized sensors are fused in a certain time frame $t=t_n$. +- Multi Frame Fusion + - In the multi frame fusion process, current fused single frame OGM in $t_n$ is fused with the previous fused single frame OGM in $t_{n-1}$. + +## I/O + +| Input topic name | Type | Description | +| ------------------ | ------------------------------------ | --------------------------------------------------------------------------------- | +| `input_ogm_topics` | list of nav_msgs::msg::OccupancyGrid | List of input topics for Occupancy Grid Maps. This parameter is given in list, so | + +| Output topic name | Type | Description | +| ----------------------------- | ---------------------------- | ----------------------------------------------------------------------------- | +| `~/output/occupancy_grid_map` | nav_msgs::msg::OccupancyGrid | Output topic name of the fused Occupancy Grid Map. | +| `~/debug/single_frame_map` | nav_msgs::msg::OccupancyGrid | (debug topic) Output topic name of the single frame fused Occupancy Grid Map. | + +## Parameters + +Synchronized OGM fusion node parameters are shown in the following table. Main parameters to be considered in the fusion node is shown as bold. + +| Ros param name | Sample value | Description | +| --------------------------- | -------------------- | ------------------------------------------------------------- | +| **input_ogm_topics** | ["topic1", "topic2"] | List of input topics for Occupancy Grid Maps | +| **input_ogm_reliabilities** | [0.8, 0.2] | Weights for the reliability of each input topic | +| **fusion_method** | "overwrite" | Method of fusion ("overwrite", "log-odds", "dempster-shafer") | +| match_threshold_sec | 0.01 | Matching threshold in milliseconds | +| timeout_sec | 0.1 | Timeout duration in seconds | +| input_offset_sec | [0.0, 0.0] | Offset time in seconds for each input topic | +| map*frame* | "map" | Frame name for the fused map | +| base*link_frame* | "base_link" | Frame name for the base link | +| grid*map_origin_frame* | "base_link" | Frame name for the origin of the grid map | +| fusion_map_length_x | 100.0 | Length of the fused map along the X-axis | +| fusion_map_length_y | 100.0 | Length of the fused map along the Y-axis | +| fusion_map_resolution | 0.5 | Resolution of the fused map | + +Since this node assumes that the OGMs from synchronized sensors are generated in the same time, we need to tune the `match_threshold_sec`, `timeout_sec` and `input_offset_sec` parameters to successfully fuse the OGMs. + +## Fusion methods + +For the single frame fusion, the following fusion methods are supported. + +| Fusion Method in parameter | Description | +| -------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `overwrite` | The value of the cell in the fused OGM is overwritten by the value of the cell in the OGM with the highest priority.
We set priority as `Occupied` > `Free` > `Unknown`. | +| `log-odds` | The value of the cell in the fused OGM is calculated by the log-odds ratio method, which is known as a Bayesian fusion method.
The log-odds of a probability $p$ can be written as $l_p = \log(\frac{p}{1-p})$.
And the fused log-odds is calculated by the sum of log-odds. $l_f = \Sigma l_p$ | +| `dempster-shafer` | The value of the cell in the fused OGM is calculated by the Dempster-Shafer theory[1]. This is also popular method to handle multiple evidences. This package applied conflict escape logic in [2] for the performance. See references for the algorithm details. | + +For the multi frame fusion, currently only supporting `log-odds` fusion method. + +## How to use + +### launch fusion node + +The minimum node launch will be like the following. + +```xml + + + + + + + + + + +``` + +### (Optional) Generate OGMs in each sensor frame + +You need to generate OGMs in each sensor frame before achieving grid map fusion. + +`probabilistic_occupancy_grid_map` package supports to generate OGMs for the each from the point cloud data. + +
+ Example launch.xml (click to expand) + +```xml + + + + + + + + + + + + + + + +The minimum parameter for the OGM generation in each frame is shown in the following table. + +|Parameter|Description| +|--|--| +|`input/obstacle_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is segmented as the obstacle.| +|`input/raw_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is not segmented as the obstacle. | +|`output`| The output topic of the OGM. | +|`map_frame`| The tf frame for the OGM center origin. | +|`scan_origin`| The tf frame for the sensor origin. | +|`method`| The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +|`param_file`| The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +``` + +
+ +
+ +We recommend to use same `map_frame`, size and resolutions for the OGMs from synchronized sensors. +Also, remember to set `enable_single_frame_mode` and `filter_obstacle_pointcloud_by_raw_pointcloud` to `true` in the `probabilistic_occupancy_grid_map` package (you do not need to set these parameters if you use the above example config file). + +
+ +### Run both OGM generation node and fusion node + +We prepared the launch file to run both OGM generation node and fusion node in [`grid_map_fusion_with_synchronized_pointclouds.launch.py`](launch/grid_map_fusion_with_synchronized_pointclouds.launch.py) + +You can include this launch file like the following. + +```xml + + + + + + + + + + +``` + +The minimum parameter for the launch file is shown in the following table. + +| Parameter | Description | +| -------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `output` | The output topic of the finally fused OGM. | +| `method` | The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +| `fusion_config_file` | The parameter file for the grid map fusion. See [example parameter file](config/grid_map_fusion.param.yaml) | +| `ogm_config_file` | The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +## References + +- [1] Dempster, A. P., Laird, N. M., & Rubin, D. B. (1977). Maximum likelihood from incomplete data via the EM algorithm. Journal of the Royal Statistical Society. Series B (Methodological), 39(1), 1-38. +- [2] diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp new file mode 100644 index 0000000000000..c41c809a92b0c --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -0,0 +1,47 @@ +// 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 "probabilistic_occupancy_grid_map/cost_value.hpp" + +#include + +// Test the CostTranslationTable and InverseCostTranslationTable functions +using occupancy_cost_value::cost_translation_table; +using occupancy_cost_value::inverse_cost_translation_table; + +TEST(CostTranslationTableTest, TestRange) +{ + // Test the value range of CostTranslationTable + for (int i = 0; i < 256; i++) { + EXPECT_GE(cost_translation_table[i], 1); + EXPECT_LE(cost_translation_table[i], 99); + } + + // Test the value range of InverseCostTranslationTable + for (int i = 1; i < 100; i++) { + EXPECT_GE(inverse_cost_translation_table[i], 0); + EXPECT_LE(inverse_cost_translation_table[i], 255); + } +} + +TEST(CostTranslationTableTest, TestConversion) +{ + // Test the forward and inverse conversion of 0, 128, and 255 + EXPECT_EQ(cost_translation_table[0], 1); + EXPECT_EQ(cost_translation_table[128], 50); + EXPECT_EQ(cost_translation_table[255], 99); + EXPECT_EQ(inverse_cost_translation_table[1], 2); + EXPECT_EQ(inverse_cost_translation_table[50], 128); + EXPECT_EQ(inverse_cost_translation_table[99], 255); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp new file mode 100644 index 0000000000000..6b3dc8a2ebcef --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -0,0 +1,76 @@ +// 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 "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" + +#include + +// Test the log-odds update rule +TEST(FusionPolicyTest, TestLogOddsUpdateRule) +{ + using fusion_policy::log_odds_fusion::logOddsFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(logOddsFusion(case1_1), OCCUPIED); + EXPECT_LE(logOddsFusion(case1_2), FREE); + EXPECT_NEAR(logOddsFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(logOddsFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(logOddsFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(logOddsFusion(case3_1), UNKNOWN, EPSILON); +} + +// Test the dempster-shafer update rule +TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) +{ + using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(dempsterShaferFusion(case1_1), OCCUPIED); + EXPECT_LE(dempsterShaferFusion(case1_2), FREE); + EXPECT_NEAR(dempsterShaferFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(dempsterShaferFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(dempsterShaferFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(dempsterShaferFusion(case3_1), UNKNOWN, EPSILON); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py new file mode 100644 index 0000000000000..c15daaf886b6f --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py @@ -0,0 +1,255 @@ +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest + +from geometry_msgs.msg import TransformStamped +import launch +import launch.actions +from launch_ros.substitutions import FindPackageShare +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.tools +from nav_msgs.msg import OccupancyGrid +import numpy as np +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +PACKAGE_NAME = "probabilistic_occupancy_grid_map" +INPUT_TOPICS = ["/topic1", "/topic2"] +DEBUG_OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/debug/single_frame_map" +OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/output/occupancy_grid_map" + +FREE_VALUE = 1 +UNKNOWN_VALUE = 50 +OCCUPIED_VALUE = 99 + + +# test launcher to launch grid map fusion node +@pytest.mark.launch_test +def generate_test_description(): + """Launch file test description. + + Returns: + _type_: launch.LaunchDescription + """ + # get launch file path + launch_file_path = ( + FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME) + + "/launch/synchronized_occupancy_grid_map_fusion.launch.xml" + ) + # use default launch arguments and parms + launch_args = [] + # action to include launch file + test_launch_file = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.AnyLaunchDescriptionSource(launch_file_path), + launch_arguments=launch_args, + ) + + return launch.LaunchDescription( + [ + test_launch_file, + launch_testing.actions.ReadyToTest(), + ] + ) + + +# util functions +def create_nav_msgs_occupancy_grid_msg(fill_value: int = 0, stamp: rclpy.time.Time = None): + """Create nav_msgs occupancy grid message. + + Args: + fill_value (int, optional): fill value. Defaults to 0. + + Returns: + OccupancyGrid: nav_msgs occupancy grid message + """ + msg = OccupancyGrid() + msg.header.stamp = rclpy.clock.Clock().now().to_msg() if stamp is None else stamp.to_msg() + msg.header.frame_id = "map" + msg.info.resolution = 0.5 # 0.5m x 0.5m + msg.info.width = 200 # 100m x 100m + msg.info.height = 200 + msg.info.origin.position.x = -msg.info.width * msg.info.resolution / 2.0 + msg.info.origin.position.y = -msg.info.height * msg.info.resolution / 2.0 + msg.info.origin.position.z = 0.0 + msg.info.origin.orientation.x = 0.0 + msg.info.origin.orientation.y = 0.0 + msg.info.origin.orientation.z = 0.0 + msg.info.origin.orientation.w = 1.0 + msg.data = [fill_value] * msg.info.width * msg.info.height + return msg + + +def parse_ogm_msg(msg: OccupancyGrid): + """Parse nav_msgs occupancy grid message. + + Args: + msg (OccupancyGrid): nav_msgs occupancy grid message + + Returns: + np.ndarray: occupancy grid map + """ + ogm = np.array(msg.data).reshape((msg.info.height, msg.info.width)) + return ogm + + +# dummy tf broadcaster +def generate_static_transform_msg(stamp: rclpy.time.Time = None): + """Generate static transform message from base_link to map. + + Returns: + TransformStamped: static transform message + """ + msg = TransformStamped() + if stamp is None: + msg.header.stamp = rclpy.clock.Clock().now().to_msg() + else: + msg.header.stamp = stamp.to_msg() + msg.header.frame_id = "map" + msg.child_frame_id = "base_link" + msg.transform.translation.x = 0.0 + msg.transform.translation.y = 0.0 + msg.transform.translation.z = 0.0 + msg.transform.rotation.x = 0.0 + msg.transform.rotation.y = 0.0 + msg.transform.rotation.z = 0.0 + msg.transform.rotation.w = 1.0 + return msg + + +# --- TestSynchronizedOGMFusion --- +# 1. test free ogm and free ogm input fusion +# 2. test occupied ogm and occupied ogm input fusion +# 3. test unknown ogm and free ogm input fusion +# 4. test unknown ogm and occupied ogm input fusion +# 5. test free ogm and occupied ogm input fusion +class TestSynchronizedOGMFusion(unittest.TestCase): + @classmethod + def setUpClass(cls): + # init ROS at once + rclpy.init() + + @classmethod + def tearDownClass(cls): + # shutdown ROS at once + rclpy.shutdown() + + def setUp(self): + # called when each test started + self.node = rclpy.create_node("grid_map_fusion_node_test_node") + # send static transform from map to base_link + tf_msg = generate_static_transform_msg() + self.tf_broadcaster = StaticTransformBroadcaster(self.node) + self.tf_broadcaster.sendTransform(tf_msg) + + def tearDown(self): + # called when each test finished + self.node.destroy_node() + + def callback(self, msg: OccupancyGrid): + self.msg_buffer.append(msg) + print("callback", len(self.msg_buffer)) + + def get_newest_ogm_value(self): + return np.mean(self.msg_buffer[-1].data) + + # util functions test 1~3 + def create_pub_sub(self): + # create publisher + pub1 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[0], 10) + pub2 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[1], 10) + + # create subscriber for debug output + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.msg_buffer = [] + # subscribe to occupancy grid with buffer + sub = self.node.create_subscription( + OccupancyGrid, DEBUG_OUTPUT_TOPIC, self.callback, qos_profile=sensor_qos + ) + return [pub1, pub2], sub + + # test functions + def test_same_ogm_fusion(self): + """Test 1~3. + + Expected output: same ogm. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() # pubs have two publishers + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + + for fill_value in fill_values: + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value) + # publish free/occupied/unknown ogm + pubs[0].publish(ogm) + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("same ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + def test_unknown_fusion(self): + """Test unknown ogm and free ogm input fusion. + + Expected output: free, occupied, unknown. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + now = rclpy.clock.Clock().now() + unknown_ogm = create_nav_msgs_occupancy_grid_msg(fill_value=UNKNOWN_VALUE, stamp=now) + + for fill_value in fill_values: + # publish unknown ogm + pubs[0].publish(unknown_ogm) + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value, stamp=now) + # publish ogm + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("unknown ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # check exit code + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 367dcee807e7b..4bc3dca0a67bd 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +// autoware +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" + #include #include @@ -21,9 +24,6 @@ #include #include -// autoware -#include "utils/utils.hpp" - // create pointcloud function pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) { diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp similarity index 94% rename from perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp rename to perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp index b5f2005a84baf..0d2f6fd10a6b6 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ -#define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #include "rclcpp/logger.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" @@ -115,4 +115,4 @@ class RadarFusionToDetectedObject }; } // namespace radar_fusion_to_detected_object -#endif // RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#endif // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ diff --git a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp similarity index 89% rename from perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp rename to perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp index 1a90e9f6bd046..837ef45c65c60 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp @@ -13,13 +13,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ -#define RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" -#include "radar_fusion_to_detected_object.hpp" +#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" @@ -89,4 +89,4 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node } // namespace radar_fusion_to_detected_object -#endif // RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#endif // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ diff --git a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json b/perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json similarity index 97% rename from perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json rename to perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json index 688414df56c1e..69c83503a2220 100644 --- a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json +++ b/perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Radar Fusion to Detected Object Node", "type": "object", "definitions": { - "radar_fusion_to_detected_object": { + "radar_object_fusion_to_detected_object": { "type": "object", "properties": { "node_params": { @@ -104,7 +104,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/radar_fusion_to_detected_object" + "$ref": "#/definitions/radar_object_fusion_to_detected_object" } }, "required": ["ros__parameters"] diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 75684d51039fd..43057e685736c 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -13,7 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_fusion_to_detected_object.hpp" +#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" #include #include diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index b233bdf31adc6..23e6d43422fa1 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -13,8 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp" - +#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 54ef7b047bf17..7c1b98c10d14b 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(nlohmann_json REQUIRED) # for debug +find_package(glog REQUIRED) include_directories( SYSTEM @@ -32,6 +33,7 @@ target_link_libraries(radar_object_tracker_node Eigen3::Eigen yaml-cpp nlohmann_json::nlohmann_json # for debug + glog::glog ) rclcpp_components_register_node(radar_object_tracker_node diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index ad71e613c1a18..6939c54b5be75 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -17,6 +17,7 @@ eigen kalman_filter lanelet2_extension + libgoogle-glog-dev mussp nlohmann-json-dev object_recognition_utils diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 6d801302ab6c5..e4b394256101d 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -23,6 +23,7 @@ #include +#include #include #include @@ -194,6 +195,10 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("radar_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index c29d8ee424549..fb8f117c82245 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -1,6 +1,6 @@ # radar_tracks_msgs_converter -This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). +This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). - Calculation cost is O(n). - n: The number of radar objects @@ -20,10 +20,14 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe - `update_rate_hz` (double): The update rate [hz]. - Default parameter is 20.0 -- `new_frame_id` (string): The header frame of output topic. +- `new_frame_id` (string): The header frame of the output topic. - Default parameter is "base_link" -- `use_twist_compensation` (bool): If the parameter is true, then the twist of output objects' topic is compensated by ego vehicle motion. +- `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion. - Default parameter is "false" +- `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. + - Default parameter is "false" +- `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s]. + - Default parameter is 1.0 ## Note @@ -42,4 +46,5 @@ Label id is defined as below. | PEDESTRIAN | 32007 | 7 | - [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg): additional vendor-specific classifications are permitted starting from 32000. + - [Autoware objects label](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl) diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 351410161d8b2..53a2a8655b9dd 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-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. @@ -43,6 +43,7 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using nav_msgs::msg::Odometry; +using radar_msgs::msg::RadarTrack; using radar_msgs::msg::RadarTracks; class RadarTracksMsgsConverterNode : public rclcpp::Node @@ -55,6 +56,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node double update_rate_hz{}; std::string new_frame_id{}; bool use_twist_compensation{}; + bool use_twist_yaw_compensation{}; + double static_object_speed_threshold{}; }; private: @@ -94,6 +97,20 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance(); TrackedObjects convertRadarTrackToTrackedObjects(); DetectedObjects convertTrackedObjectsToDetectedObjects(TrackedObjects & objects); + geometry_msgs::msg::Vector3 compensateVelocitySensorPosition( + const radar_msgs::msg::RadarTrack & radar_track); + geometry_msgs::msg::Vector3 compensateVelocityEgoMotion( + const geometry_msgs::msg::Vector3 & velocity_in, + const geometry_msgs::msg::Point & position_from_veh); + bool isStaticObject( + const radar_msgs::msg::RadarTrack & radar_track, + const geometry_msgs::msg::Vector3 & compensated_velocity); + std::array convertPoseCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track); + std::array convertTwistCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track); + std::array convertAccelerationCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track); uint8_t convertClassification(const uint16_t classification); }; diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index c9f4a31354247..a6a3f394b1f40 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -5,6 +5,8 @@ + + @@ -13,5 +15,7 @@ + + diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index d8b5599cc1ca7..323e4bf7788c5 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-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. @@ -81,6 +81,10 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); node_param_.use_twist_compensation = declare_parameter("use_twist_compensation", false); + node_param_.use_twist_yaw_compensation = + declare_parameter("use_twist_yaw_compensation", false); + node_param_.static_object_speed_threshold = + declare_parameter("static_object_speed_threshold", 1.0); // Subscriber sub_radar_ = create_subscription( @@ -125,6 +129,8 @@ rcl_interfaces::msg::SetParametersResult RadarTracksMsgsConverterNode::onSetPara update_param(params, "update_rate_hz", p.update_rate_hz); update_param(params, "new_frame_id", p.new_frame_id); update_param(params, "use_twist_compensation", p.use_twist_compensation); + update_param(params, "use_twist_yaw_compensation", p.use_twist_yaw_compensation); + update_param(params, "static_object_speed_threshold", p.static_object_speed_threshold); } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; @@ -192,13 +198,32 @@ DetectedObjects RadarTracksMsgsConverterNode::convertTrackedObjectsToDetectedObj return detected_objects; } +bool RadarTracksMsgsConverterNode::isStaticObject( + const radar_msgs::msg::RadarTrack & radar_track, + const geometry_msgs::msg::Vector3 & compensated_velocity) +{ + if (!(node_param_.use_twist_compensation && odometry_data_)) { + return false; + } + + // Calculate azimuth angle of the object in the vehicle coordinate + const double sensor_yaw = tf2::getYaw(transform_->transform.rotation); + const double radar_azimuth = std::atan2(radar_track.position.y, radar_track.position.x); + const double azimuth = radar_azimuth + sensor_yaw; + + // Calculate longitudinal speed + const double longitudinal_speed = + compensated_velocity.x * std::cos(azimuth) + compensated_velocity.y * std::sin(azimuth); + + // Check if the object is static + return std::abs(longitudinal_speed) < node_param_.static_object_speed_threshold; +} + TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() { TrackedObjects tracked_objects; tracked_objects.header = radar_data_->header; tracked_objects.header.frame_id = node_param_.new_frame_id; - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; for (auto & radar_track : radar_data_->tracks) { TrackedObject tracked_object; @@ -209,40 +234,10 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() tracked_object.shape.type = Shape::BOUNDING_BOX; tracked_object.shape.dimensions = radar_track.size; - // kinematics setting - TrackedObjectKinematics kinematics; - kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; - kinematics.is_stationary = false; - - geometry_msgs::msg::Vector3 compensated_velocity{}; - { - double rotate_yaw = tf2::getYaw(transform_->transform.rotation); - const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; - compensated_velocity.x = vel.x * std::cos(rotate_yaw) - vel.y * std::sin(rotate_yaw); - compensated_velocity.y = vel.x * std::sin(rotate_yaw) + vel.y * std::cos(rotate_yaw); - compensated_velocity.z = radar_track.velocity.z; - } - - if (node_param_.use_twist_compensation) { - if (odometry_data_) { - compensated_velocity.x += odometry_data_->twist.twist.linear.x; - compensated_velocity.y += odometry_data_->twist.twist.linear.y; - compensated_velocity.z += odometry_data_->twist.twist.linear.z; - } else { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Odometry data is not coming"); - } - } - kinematics.twist_with_covariance.twist.linear.x = std::sqrt( - compensated_velocity.x * compensated_velocity.x + - compensated_velocity.y * compensated_velocity.y); - // Pose conversion geometry_msgs::msg::PoseStamped radar_pose_stamped{}; radar_pose_stamped.pose.position = radar_track.position; - double yaw = tier4_autoware_utils::normalizeRadian( - std::atan2(compensated_velocity.y, compensated_velocity.x)); - geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; if (transform_ == nullptr) { RCLCPP_ERROR_THROTTLE( @@ -250,50 +245,47 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() return tracked_objects; } tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); + const auto & position_from_veh = transformed_pose_stamped.pose.position; + + // Velocity conversion + // 1: Compensate radar coordinate + // radar track velocity is defined in the radar coordinate + // compensate radar coordinate to vehicle coordinate + auto compensated_velocity = compensateVelocitySensorPosition(radar_track); + // 2: Compensate ego motion + if (node_param_.use_twist_compensation && odometry_data_) { + compensated_velocity = compensateVelocityEgoMotion(compensated_velocity, position_from_veh); + } else if (node_param_.use_twist_compensation && !odometry_data_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Odometry data is not available. Radar track velocity will not be compensated."); + } + + // yaw angle (vehicle heading) is obtained from ground velocity + double yaw = tier4_autoware_utils::normalizeRadian( + std::atan2(compensated_velocity.y, compensated_velocity.x)); + + // kinematics setting + TrackedObjectKinematics kinematics; + kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; + kinematics.is_stationary = isStaticObject(radar_track, compensated_velocity); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; + kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(radar_track); + // velocity of object is defined in the object coordinate + // heading is obtained from ground velocity kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - { - auto & pose_cov = kinematics.pose_with_covariance.covariance; - auto & radar_position_cov = radar_track.position_covariance; - pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X]; - pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y]; - pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z]; - pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y]; - pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y]; - pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z]; - pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z]; - pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z]; - pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z]; - } - - { - auto & twist_cov = kinematics.twist_with_covariance.covariance; - auto & radar_vel_cov = radar_track.velocity_covariance; - twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X]; - twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y]; - twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z]; - twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y]; - twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y]; - twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z]; - twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z]; - twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z]; - twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z]; - } - { - auto & accel_cov = kinematics.acceleration_with_covariance.covariance; - auto & radar_accel_cov = radar_track.acceleration_covariance; - accel_cov[POSE_IDX::X_X] = radar_accel_cov[RADAR_IDX::X_X]; - accel_cov[POSE_IDX::X_Y] = radar_accel_cov[RADAR_IDX::X_Y]; - accel_cov[POSE_IDX::X_Z] = radar_accel_cov[RADAR_IDX::X_Z]; - accel_cov[POSE_IDX::Y_X] = radar_accel_cov[RADAR_IDX::X_Y]; - accel_cov[POSE_IDX::Y_Y] = radar_accel_cov[RADAR_IDX::Y_Y]; - accel_cov[POSE_IDX::Y_Z] = radar_accel_cov[RADAR_IDX::Y_Z]; - accel_cov[POSE_IDX::Z_X] = radar_accel_cov[RADAR_IDX::X_Z]; - accel_cov[POSE_IDX::Z_Y] = radar_accel_cov[RADAR_IDX::Y_Z]; - accel_cov[POSE_IDX::Z_Z] = radar_accel_cov[RADAR_IDX::Z_Z]; - } + // longitudinal velocity is the length of the velocity vector + // lateral velocity is zero, use default value + kinematics.twist_with_covariance.twist.linear.x = std::sqrt( + compensated_velocity.x * compensated_velocity.x + + compensated_velocity.y * compensated_velocity.y); + kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(radar_track); + // acceleration is zero, use default value + kinematics.acceleration_with_covariance.covariance = + convertAccelerationCovarianceMatrix(radar_track); + // fill the kinematics to the tracked object tracked_object.kinematics = kinematics; // classification @@ -307,6 +299,91 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() return tracked_objects; } +geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocitySensorPosition( + const radar_msgs::msg::RadarTrack & radar_track) +{ + // initialize compensated velocity + geometry_msgs::msg::Vector3 compensated_velocity{}; + + const double sensor_yaw = tf2::getYaw(transform_->transform.rotation); + const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; + compensated_velocity.x = vel.x * std::cos(sensor_yaw) - vel.y * std::sin(sensor_yaw); + compensated_velocity.y = vel.x * std::sin(sensor_yaw) + vel.y * std::cos(sensor_yaw); + compensated_velocity.z = vel.z; + + return compensated_velocity; +} + +geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocityEgoMotion( + const geometry_msgs::msg::Vector3 & velocity_in, + const geometry_msgs::msg::Point & position_from_veh) +{ + geometry_msgs::msg::Vector3 velocity = velocity_in; + // linear compensation + velocity.x += odometry_data_->twist.twist.linear.x; + velocity.y += odometry_data_->twist.twist.linear.y; + velocity.z += odometry_data_->twist.twist.linear.z; + if (node_param_.use_twist_yaw_compensation) { + // angular compensation + const double veh_yaw = odometry_data_->twist.twist.angular.z; + velocity.x += -position_from_veh.y * veh_yaw; + velocity.y += position_from_veh.x * veh_yaw; + } + return velocity; +} + +std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + std::array pose_covariance{}; + pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; + pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; + pose_covariance[POSE_IDX::X_Z] = radar_track.position_covariance[RADAR_IDX::X_Z]; + pose_covariance[POSE_IDX::Y_X] = radar_track.position_covariance[RADAR_IDX::X_Y]; + pose_covariance[POSE_IDX::Y_Y] = radar_track.position_covariance[RADAR_IDX::Y_Y]; + pose_covariance[POSE_IDX::Y_Z] = radar_track.position_covariance[RADAR_IDX::Y_Z]; + pose_covariance[POSE_IDX::Z_X] = radar_track.position_covariance[RADAR_IDX::X_Z]; + pose_covariance[POSE_IDX::Z_Y] = radar_track.position_covariance[RADAR_IDX::Y_Z]; + pose_covariance[POSE_IDX::Z_Z] = radar_track.position_covariance[RADAR_IDX::Z_Z]; + return pose_covariance; +} +std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + std::array twist_covariance{}; + twist_covariance[POSE_IDX::X_X] = radar_track.velocity_covariance[RADAR_IDX::X_X]; + twist_covariance[POSE_IDX::X_Y] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; + twist_covariance[POSE_IDX::X_Z] = radar_track.velocity_covariance[RADAR_IDX::X_Z]; + twist_covariance[POSE_IDX::Y_X] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; + twist_covariance[POSE_IDX::Y_Y] = radar_track.velocity_covariance[RADAR_IDX::Y_Y]; + twist_covariance[POSE_IDX::Y_Z] = radar_track.velocity_covariance[RADAR_IDX::Y_Z]; + twist_covariance[POSE_IDX::Z_X] = radar_track.velocity_covariance[RADAR_IDX::X_Z]; + twist_covariance[POSE_IDX::Z_Y] = radar_track.velocity_covariance[RADAR_IDX::Y_Z]; + twist_covariance[POSE_IDX::Z_Z] = radar_track.velocity_covariance[RADAR_IDX::Z_Z]; + return twist_covariance; +} +std::array RadarTracksMsgsConverterNode::convertAccelerationCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + std::array acceleration_covariance{}; + acceleration_covariance[POSE_IDX::X_X] = radar_track.acceleration_covariance[RADAR_IDX::X_X]; + acceleration_covariance[POSE_IDX::X_Y] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; + acceleration_covariance[POSE_IDX::X_Z] = radar_track.acceleration_covariance[RADAR_IDX::X_Z]; + acceleration_covariance[POSE_IDX::Y_X] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; + acceleration_covariance[POSE_IDX::Y_Y] = radar_track.acceleration_covariance[RADAR_IDX::Y_Y]; + acceleration_covariance[POSE_IDX::Y_Z] = radar_track.acceleration_covariance[RADAR_IDX::Y_Z]; + acceleration_covariance[POSE_IDX::Z_X] = radar_track.acceleration_covariance[RADAR_IDX::X_Z]; + acceleration_covariance[POSE_IDX::Z_Y] = radar_track.acceleration_covariance[RADAR_IDX::Y_Z]; + acceleration_covariance[POSE_IDX::Z_Z] = radar_track.acceleration_covariance[RADAR_IDX::Z_Z]; + return acceleration_covariance; +} + uint8_t RadarTracksMsgsConverterNode::convertClassification(const uint16_t classification) { if (classification == static_cast(RadarTrackObjectID::UNKNOWN)) { diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 5ae6002cd7c3b..8eb7a15b5a885 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -64,4 +64,5 @@ rclcpp_components_register_node(shape_estimation_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index c50d66b546213..b635631381cc3 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,11 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ---- | ------------- | --------------------------------------------------- | -| `use_corrector` | bool | true | The flag to apply rule-based filter | -| `use_filter` | bool | true | The flag to apply rule-based corrector | -| `use_vehicle_reference_yaw` | bool | true | The flag to use vehicle reference yaw for corrector | +{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml new file mode 100644 index 0000000000000..253516fffe0d4 --- /dev/null +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + use_corrector: true + use_filter: true + use_vehicle_reference_yaw: false + use_vehicle_reference_shape_size: false + use_boost_bbox_optimizer: false diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp new file mode 100644 index 0000000000000..ec1a2b8a42973 --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp @@ -0,0 +1,40 @@ +// Copyright 2018 Autoware Foundation. 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 SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#define SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ + +#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "utils.hpp" + +class BicycleCorrector : public VehicleCorrector +{ +public: + explicit BicycleCorrector(const bool use_reference_yaw = false) + : VehicleCorrector(use_reference_yaw) + { + corrector_utils::CorrectionBBParameters params; + params.min_width = 0.3; + params.max_width = 1.2; + params.default_width = (params.min_width + params.max_width) * 0.5; + params.min_length = 1.0; + params.max_length = 2.8; + params.default_length = (params.min_length + params.max_length) * 0.5; + setParams(params); + } + + ~BicycleCorrector() = default; +}; + +#endif // SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp index d52bdc21f916f..e4efc17181e52 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp @@ -15,6 +15,7 @@ #ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ #define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ +#include "shape_estimation/corrector/bicycle_corrector.hpp" #include "shape_estimation/corrector/bus_corrector.hpp" #include "shape_estimation/corrector/car_corrector.hpp" #include "shape_estimation/corrector/corrector_interface.hpp" diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 8e90e3ea57cc0..65d1944417cc0 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -1,18 +1,16 @@ - - + + + - - - - + diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 9577b1e2d3197..e89eaac0a6db0 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -120,6 +120,8 @@ bool ShapeEstimator::applyCorrector( corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); } else if (label == Label::TRAILER) { corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); + } else if (label == Label::MOTORCYCLE || label == Label::BICYCLE) { + corrector_ptr.reset(new BicycleCorrector(use_reference_yaw)); } else { corrector_ptr.reset(new NoCorrector); } diff --git a/perception/shape_estimation/schema/shape_estimation.schema.json b/perception/shape_estimation/schema/shape_estimation.schema.json new file mode 100644 index 0000000000000..d81bfa636a923 --- /dev/null +++ b/perception/shape_estimation/schema/shape_estimation.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Shape Estimation Node", + "type": "object", + "definitions": { + "shape_estimation": { + "type": "object", + "properties": { + "use_corrector": { + "type": "boolean", + "description": "The flag to apply rule-based corrector.", + "default": "true" + }, + "use_filter": { + "type": "boolean", + "description": "The flag to apply rule-based filter", + "default": "true" + }, + "use_vehicle_reference_yaw": { + "type": "boolean", + "description": "The flag to use vehicle reference yaw for corrector", + "default": "false" + }, + "use_vehicle_reference_shape_size": { + "type": "boolean", + "description": "The flag to use vehicle reference shape size", + "default": "false" + }, + "use_boost_bbox_optimizer": { + "type": "boolean", + "description": "The flag to use boost bbox optimizer", + "default": "false" + } + }, + "required": [ + "use_corrector", + "use_filter", + "use_vehicle_reference_yaw", + "use_vehicle_reference_shape_size", + "use_boost_bbox_optimizer" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/shape_estimation" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 987cf8106c99e..624ca604d8fbf 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -42,11 +42,11 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option "input", rclcpp::QoS{1}, std::bind(&ShapeEstimationNode::callback, this, _1)); pub_ = create_publisher("objects", rclcpp::QoS{1}); - bool use_corrector = declare_parameter("use_corrector", true); - bool use_filter = declare_parameter("use_filter", true); - use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw", true); - use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size", true); - bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer", false); + bool use_corrector = declare_parameter("use_corrector"); + bool use_filter = declare_parameter("use_filter"); + use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); + use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); + bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); diff --git a/perception/tensorrt_yolo/schema/tensortt_yolo.json b/perception/tensorrt_yolo/schema/tensortt_yolo.json new file mode 100644 index 0000000000000..0b4724078c994 --- /dev/null +++ b/perception/tensorrt_yolo/schema/tensortt_yolo.json @@ -0,0 +1,108 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for tensorrt_yolo", + "type": "object", + "definitions": { + "tensorrt_yolo": { + "type": "object", + "properties": { + "num_anchors": { + "type": "number", + "default": [ + 10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, + 156.0, 198.0, 373.0, 326.0 + ], + "description": "The anchors to create bounding box candidates." + }, + "scale_x_y": { + "type": "number", + "default": [1.0, 1.0, 1.0], + "description": "scale parameter to eliminate grid sensitivity." + }, + "score_thresh": { + "type": "number", + "default": 0.1, + "description": "If the objectness score is less than this value, the object is ignored in yolo layer." + }, + "iou_thresh": { + "type": "number", + "default": 0.45, + "description": "The iou threshold for NMS method." + }, + "detections_per_im": { + "type": "number", + "default": 100, + "description": " The maximum detection number for one frame." + }, + "use_darknet_layer": { + "type": "boolean", + "default": true, + "description": "The flag to use yolo layer in darknet." + }, + "ignore_thresh": { + "type": "number", + "default": 0.5, + "description": "If the output score is less than this value, this object is ignored." + }, + "onnx_file": { + "type": "string", + "description": "The onnx file name for yolo model." + }, + "engine_file": { + "type": "string", + "description": "The tensorrt engine file name for yolo model." + }, + "label_file": { + "type": "string", + "description": "The label file with label names for detected objects written on it." + }, + "calib_image_directory": { + "type": "string", + "description": "The directory name including calibration images for int8 inference." + }, + "calib_cache_file": { + "type": "string", + "description": "The calibration cache file for int8 inference." + }, + "mode": { + "type": "string", + "default": "FP32", + "description": "The inference mode: FP32, FP16, INT8." + }, + "gpu_id": { + "type": "number", + "default": 0, + "description": "GPU device ID that runs the model." + } + }, + "required": [ + "num_anchors", + "scale_x_y", + "score_thresh", + "iou_thresh", + "detections_per_im", + "use_darknet_layer", + "ignore_thresh", + "onnx_file", + "engine_file", + "label_file", + "calib_image_directory", + "calib_cache_file", + "mode", + "gpu_id" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/tensorrt_yolo" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/tensorrt_yolo/src/nodelet.cpp b/perception/tensorrt_yolo/src/nodelet.cpp index dcb65114ad88f..fdcd8bf12db72 100644 --- a/perception/tensorrt_yolo/src/nodelet.cpp +++ b/perception/tensorrt_yolo/src/nodelet.cpp @@ -50,9 +50,9 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) std::string label_file = declare_parameter("label_file", ""); std::string calib_image_directory = declare_parameter("calib_image_directory", ""); std::string calib_cache_file = declare_parameter("calib_cache_file", ""); - std::string mode = declare_parameter("mode", "FP32"); - gpu_device_id_ = declare_parameter("gpu_id", 0); - yolo_config_.num_anchors = declare_parameter("num_anchors", 3); + std::string mode = declare_parameter("mode"); + gpu_device_id_ = declare_parameter("gpu_id"); + yolo_config_.num_anchors = declare_parameter("num_anchors"); auto anchors = declare_parameter( "anchors", std::vector{ 10, 13, 16, 30, 33, 23, 30, 61, 62, 45, 59, 119, 116, 90, 156, 198, 373, 326}); @@ -61,11 +61,11 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) auto scale_x_y = declare_parameter("scale_x_y", std::vector{1.0, 1.0, 1.0}); std::vector scale_x_y_float(scale_x_y.begin(), scale_x_y.end()); yolo_config_.scale_x_y = scale_x_y_float; - yolo_config_.score_thresh = declare_parameter("score_thresh", 0.1); - yolo_config_.iou_thresh = declare_parameter("iou_thresh", 0.45); - yolo_config_.detections_per_im = declare_parameter("detections_per_im", 100); - yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer", true); - yolo_config_.ignore_thresh = declare_parameter("ignore_thresh", 0.5); + yolo_config_.score_thresh = declare_parameter("score_thresh"); + yolo_config_.iou_thresh = declare_parameter("iou_thresh"); + yolo_config_.detections_per_im = declare_parameter("detections_per_im"); + yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer"); + yolo_config_.ignore_thresh = declare_parameter("ignore_thresh"); if (!yolo::set_cuda_device(gpu_device_id_)) { RCLCPP_ERROR(this->get_logger(), "Given GPU not exist or suitable"); diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 5e46b9403462d..9c8e5a321d9be 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(autoware_cmake REQUIRED) +find_package(glog REQUIRED) autoware_package() @@ -31,6 +32,7 @@ ament_auto_add_library(decorative_tracker_merger_node SHARED target_link_libraries(decorative_tracker_merger_node Eigen3::Eigen + glog::glog ) rclcpp_components_register_node(decorative_tracker_merger_node diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp index bc6dfef9b80bf..951be5d45d508 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -18,6 +18,8 @@ #define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ // #include +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index d74a8449b20e6..58184c090a1e1 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs eigen + libgoogle-glog-dev mussp object_recognition_utils rclcpp diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index 47a333691eabf..d2bc643a8eec1 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -20,6 +20,8 @@ #include +#include + #include #include @@ -85,6 +87,10 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("decorative_object_merger_node"); + google::InstallFailureSignalHandler(); + // Subscriber sub_main_objects_ = create_subscription( "input/main_object", rclcpp::QoS{1}, diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 5987bdc2d1bef..81f63538f9e22 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -253,6 +253,68 @@ double mean(const double a, const double b) return (a + b) / 2.0; } +/** + * @brief compare two tracked objects motion direction is same or not + * + * @param main_obj + * @param sub_obj + * @return true + * @return false + */ +bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + // get yaw + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + // get velocity + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj.kinematics.twist_with_covariance.twist.linear.y; + // calc velocity direction + const auto main_vyaw = std::atan2(main_vy, main_vx); + const auto sub_vyaw = std::atan2(sub_vy, sub_vx); + // get motion yaw angle + const auto main_motion_yaw = main_yaw + main_vyaw; + const auto sub_motion_yaw = sub_yaw + sub_vyaw; + // diff of motion yaw angle + const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); + const auto normalized_motion_yaw_diff = + tier4_autoware_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + // evaluate if motion yaw angle is same + constexpr double yaw_threshold = M_PI / 4.0; // 45 deg + if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { + return true; + } else { + return false; + } +} + +/** + * @brief compare two tracked objects yaw is reverted or not + * + * @param main_obj + * @param sub_obj + * @return true + * @return false + */ +bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + // get yaw + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + // calc yaw diff + const auto yaw_diff = std::fabs(main_yaw - sub_yaw); + const auto normalized_yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_diff); // -pi ~ pi + // evaluate if yaw is reverted + constexpr double yaw_threshold = M_PI / 2.0; // 90 deg + if (std::abs(normalized_yaw_diff) >= yaw_threshold) { + return true; + } else { + return false; + } +} + // object kinematics merger // currently only support velocity fusion autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( @@ -261,18 +323,29 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; // copy main object at first output_kinematics = main_obj.kinematics; + auto sub_obj_ = sub_obj; + // do not merge reverse direction objects + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + return output_kinematics; + } else if (objectsYawIsReverted(main_obj, sub_obj)) { + // revert velocity (revert pose is not necessary because it is not used in tracking) + sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0; + } + + // currently only merge vx if (policy == MergePolicy::SKIP) { return output_kinematics; } else if (policy == MergePolicy::OVERWRITE) { output_kinematics.twist_with_covariance.twist.linear.x = - sub_obj.kinematics.twist_with_covariance.twist.linear.x; + sub_obj_.kinematics.twist_with_covariance.twist.linear.x; return output_kinematics; } else if (policy == MergePolicy::FUSION) { const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; - const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; // inverse weight const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; - const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0]; double main_vx_weight, sub_vx_weight; if (main_vx_cov == 0.0) { main_vx_weight = 1.0 * 1e6; @@ -380,9 +453,21 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger( void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { - const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + // do not update velocity + return; + } else if (objectsYawIsReverted(main_obj, sub_obj)) { + // revert velocity (revert pose is not necessary because it is not used in tracking) + const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp; + return; + } else { + // update velocity + const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; + } } void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) @@ -397,6 +482,12 @@ void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & su void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) { + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + // warning + std::cerr << "[object_tracking_merger]: motion direction is different in " + "updateWholeTrackedObject function." + << std::endl; + } main_obj = sub_obj; } diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 2aecf66f8fb7b..758234f129f2a 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -52,10 +52,11 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| ----------------- | ---- | ------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | +| Name | Type | Description | +| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | if the value is `1`, cnn_classifier is used | +| `data_path` | str | packages data and artifacts directory path | +| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | ### Core Parameters diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp index e076ff5c69378..1281285ee53dd 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -66,6 +66,8 @@ class TrafficLightClassifierNodelet : public rclcpp::Node CNN = 1, }; + uint8_t classify_traffic_light_type_; + private: void connectCb(); @@ -86,6 +88,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node rclcpp::Publisher::SharedPtr traffic_signal_array_pub_; std::shared_ptr classifier_ptr_; + + double backlight_threshold_; + bool is_harsh_backlight(const cv::Mat & img) const; }; } // namespace traffic_light diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 10aa04cc585af..343531f4a00f1 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -11,8 +11,11 @@ + + + @@ -20,9 +23,11 @@ + + diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 90cca87245d22..1ebc14c4e6f93 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "traffic_light_classifier/nodelet.hpp" +#include + #include #include #include @@ -23,9 +25,13 @@ namespace traffic_light TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options) : Node("traffic_light_classifier_node", options) { + classify_traffic_light_type_ = this->declare_parameter("classify_traffic_light_type", 0); + using std::placeholders::_1; using std::placeholders::_2; is_approximate_sync_ = this->declare_parameter("approximate_sync", false); + backlight_threshold_ = this->declare_parameter("backlight_threshold"); + if (is_approximate_sync_) { approximate_sync_.reset(new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, roi_sub_)); approximate_sync_->registerCallback( @@ -94,19 +100,73 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; + std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { - output_msg.signals[i].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; + // skip if the roi is not detected + if (input_rois_msg->rois.at(i).roi.height == 0) { + break; + } + if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) { + continue; + } + output_msg.signals[images.size()].traffic_light_id = + input_rois_msg->rois.at(i).traffic_light_id; + output_msg.signals[images.size()].traffic_light_type = + input_rois_msg->rois.at(i).traffic_light_type; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; - images.emplace_back(cv_ptr->image, cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + + auto roi_img = cv_ptr->image(cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + if (is_harsh_backlight(roi_img)) { + backlight_indices.emplace_back(i); + } + images.emplace_back(roi_img); } + + output_msg.signals.resize(images.size()); if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + + // append the undetected rois as unknown + for (const auto & input_roi : input_rois_msg->rois) { + if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) { + tier4_perception_msgs::msg::TrafficSignal tlr_sig; + tlr_sig.traffic_light_id = input_roi.traffic_light_id; + tlr_sig.traffic_light_type = input_roi.traffic_light_type; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::CIRCLE; + element.confidence = 0.0; + tlr_sig.elements.push_back(element); + output_msg.signals.push_back(tlr_sig); + } + } + + for (const auto & idx : backlight_indices) { + auto & elements = output_msg.signals.at(idx).elements; + for (auto & element : elements) { + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.confidence = 0.0; + } + } + output_msg.header = input_image_msg->header; traffic_signal_array_pub_->publish(output_msg); } +bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const +{ + cv::Mat y_cr_cb; + cv::cvtColor(img, y_cr_cb, cv::COLOR_RGB2YCrCb); + + const cv::Scalar mean_values = cv::mean(y_cr_cb); + const double intensity = (mean_values[0] - 112.5) / 112.5; + + return backlight_threshold_ <= intensity; +} + } // namespace traffic_light #include diff --git a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp index f2f1f17be6894..5c89c76a11833 100644 --- a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp +++ b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp @@ -115,6 +115,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node void detectionMatch( std::map & id2expectRoi, std::map & id2detections, TrafficLightRoiArray & out_rois); + /** * @brief convert image message to cv::Mat * @@ -137,7 +138,8 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @return true succeed * @return false failed */ - bool readLabelFile(const std::string & filepath, int & tlr_id, int & num_class); + bool readLabelFile( + const std::string & filepath, std::vector & tlr_label_id_, int & num_class); // variables image_transport::SubscriberFilter image_sub_; @@ -162,7 +164,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node bool is_approximate_sync_; double score_thresh_; - int tlr_id_; + std::vector tlr_label_id_; int batch_size_; std::unique_ptr trt_yolox_; diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 86e77bc31cd71..8037dc5472fbe 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -73,7 +73,7 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( float nms_threshold = declare_parameter("fine_detector_nms_thresh", 0.65); is_approximate_sync_ = this->declare_parameter("approximate_sync", false); - if (!readLabelFile(label_path, tlr_id_, num_class)) { + if (!readLabelFile(label_path, tlr_label_id_, num_class)) { RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } @@ -157,6 +157,7 @@ void TrafficLightFineDetectorNodelet::callback( tensorrt_yolox::ObjectArrays inference_results; std::vector lts; std::vector roi_ids; + for (size_t roi_i = 0; roi_i < rough_roi_msg->rois.size(); roi_i++) { const auto & rough_roi = rough_roi_msg->rois[roi_i]; cv::Point lt(rough_roi.roi.x_offset, rough_roi.roi.y_offset); @@ -178,7 +179,7 @@ void TrafficLightFineDetectorNodelet::callback( trt_yolox_->doMultiScaleInference(original_image, inference_results, rois); for (size_t batch_i = 0; batch_i < true_batch_size; batch_i++) { for (const tensorrt_yolox::Object & detection : inference_results[batch_i]) { - if (detection.score < score_thresh_ || detection.type != tlr_id_) { + if (detection.score < score_thresh_) { continue; } cv::Point lt_roi( @@ -190,6 +191,7 @@ void TrafficLightFineDetectorNodelet::callback( det.y_offset = lt_roi.y; det.width = rb_roi.x - lt_roi.x; det.height = rb_roi.y - lt_roi.y; + det.type = detection.type; id2detections[roi_ids[batch_i]].push_back(det); } } @@ -270,13 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch( } out_rois.rois.clear(); - for (const auto & p : bestDetections) { + std::vector invalid_roi_id; + for (const auto & [tlr_id, roi] : id2expectRoi) { + // if matches, update the roi info + if (!bestDetections.count(tlr_id)) { + invalid_roi_id.emplace_back(tlr_id); + continue; + } + TrafficLightRoi tlr; + tlr.traffic_light_id = tlr_id; + const auto & object = bestDetections.at(tlr_id); + tlr.traffic_light_type = roi.traffic_light_type; + tlr.roi.x_offset = object.x_offset; + tlr.roi.y_offset = object.y_offset; + tlr.roi.width = object.width; + tlr.roi.height = object.height; + out_rois.rois.push_back(tlr); + } + // append undetected rois at the end + for (const auto & id : invalid_roi_id) { TrafficLightRoi tlr; - tlr.traffic_light_id = p.first; - tlr.roi.x_offset = p.second.x_offset; - tlr.roi.y_offset = p.second.y_offset; - tlr.roi.width = p.second.width; - tlr.roi.height = p.second.height; + tlr.traffic_light_id = id; + tlr.traffic_light_type = id2expectRoi[id].traffic_light_type; out_rois.rois.push_back(tlr); } } @@ -318,9 +335,8 @@ bool TrafficLightFineDetectorNodelet::fitInFrame( } bool TrafficLightFineDetectorNodelet::readLabelFile( - const std::string & filepath, int & tlr_id, int & num_class) + const std::string & filepath, std::vector & tlr_label_id_, int & num_class) { - tlr_id = -1; std::ifstream labelsFile(filepath); if (!labelsFile.is_open()) { RCLCPP_ERROR(this->get_logger(), "Could not open label file. [%s]", filepath.c_str()); @@ -329,13 +345,13 @@ bool TrafficLightFineDetectorNodelet::readLabelFile( std::string label; int idx = 0; while (getline(labelsFile, label)) { - if (label == "traffic_light") { - tlr_id = idx; + if (label == "traffic_light" || label == "pedestrian_traffic_light") { + tlr_label_id_.push_back(idx); } idx++; } num_class = idx; - return tlr_id != -1; + return tlr_label_id_.size() != 0; } } // namespace traffic_light diff --git a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml index 2b8ff4aa737b7..2eb671c1f5150 100644 --- a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml +++ b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml @@ -6,3 +6,5 @@ max_vibration_width: 0.5 # -0.25 ~ 0.25 m max_vibration_depth: 0.5 # -0.25 ~ 0.25 m max_detection_range: 200.0 + car_traffic_light_max_angle_range: 40.0 + pedestrian_traffic_light_max_angle_range: 80.0 diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index e46431a7b199e..0d2b7ffeb7597 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -58,6 +58,8 @@ class MapBasedDetector : public rclcpp::Node double max_timestamp_offset; double timestamp_sample_len; double max_detection_range; + double car_traffic_light_max_angle_range; + double pedestrian_traffic_light_max_angle_range; }; struct IdLessThan @@ -93,10 +95,14 @@ class MapBasedDetector : public rclcpp::Node std::shared_ptr all_traffic_lights_ptr_; std::shared_ptr route_traffic_lights_ptr_; + std::set pedestrian_tl_id_; + lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; + Config config_; /** * @brief Calculated the transform from map to frame_id at timestamp t diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index f0a5d7b7b1fde..5689e55dca316 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -137,6 +137,10 @@ MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options) config_.max_timestamp_offset = declare_parameter("max_timestamp_offset", 0.0); config_.timestamp_sample_len = declare_parameter("timestamp_sample_len", 0.01); config_.max_detection_range = declare_parameter("max_detection_range", 200.0); + config_.car_traffic_light_max_angle_range = + declare_parameter("car_traffic_light_max_angle_range", 40.0); + config_.pedestrian_traffic_light_max_angle_range = + declare_parameter("pedestrian_traffic_light_max_angle_range", 80.0); if (config_.max_detection_range <= 0) { RCLCPP_ERROR_STREAM( @@ -287,6 +291,11 @@ bool MapBasedDetector::getTrafficLightRoi( tier4_perception_msgs::msg::TrafficLightRoi & roi) const { roi.traffic_light_id = traffic_light.id(); + if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { + roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT; + } else { + roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT; + } // for roi.x_offset and roi.y_offset { @@ -392,7 +401,6 @@ void MapBasedDetector::mapCallback( for (auto tl_itr = all_lanelet_traffic_lights.begin(); tl_itr != all_lanelet_traffic_lights.end(); ++tl_itr) { lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; - auto lights = tl->trafficLights(); for (auto lsp : lights) { if (!lsp.isLineString()) { // traffic lights must be linestrings @@ -401,6 +409,28 @@ void MapBasedDetector::mapCallback( all_traffic_lights_ptr_->insert(static_cast(lsp)); } } + + auto crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); + for (const auto & tl : lanelet::utils::query::autowareTrafficLights(crosswalk_lanelets)) { + for (const auto & lsp : tl->trafficLights()) { + if (lsp.isLineString()) { // traffic lights must be linestrings + pedestrian_tl_id_.insert(lsp.id()); + } + } + } + + // crosswalk + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); } void MapBasedDetector::routeCallback( @@ -436,6 +466,34 @@ void MapBasedDetector::routeCallback( route_traffic_lights_ptr_->insert(static_cast(lsp)); } } + + // crosswalk trafficlights + lanelet::ConstLanelets conflicting_crosswalks; + pedestrian_tl_id_.clear(); + + for (const auto & route_lanelet : route_lanelets) { + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflict_lls = + overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); + for (const auto & lanelet : conflict_lls) { + conflicting_crosswalks.push_back(lanelet); + } + } + std::vector crosswalk_lanelet_traffic_lights = + lanelet::utils::query::autowareTrafficLights(conflicting_crosswalks); + for (auto tl_itr = crosswalk_lanelet_traffic_lights.begin(); + tl_itr != crosswalk_lanelet_traffic_lights.end(); ++tl_itr) { + lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; + + auto lights = tl->trafficLights(); + for (auto lsp : lights) { + if (!lsp.isLineString()) { // traffic lights must be linestrings + continue; + } + route_traffic_lights_ptr_->insert(static_cast(lsp)); + pedestrian_tl_id_.insert(lsp.id()); + } + } } void MapBasedDetector::getVisibleTrafficLights( @@ -445,12 +503,19 @@ void MapBasedDetector::getVisibleTrafficLights( std::vector & visible_traffic_lights) const { for (const auto & traffic_light : all_traffic_lights) { - // some "Traffic Light" are actually not traffic lights if ( traffic_light.hasAttribute("subtype") == false || traffic_light.attribute("subtype").value() == "solid") { continue; } + // set different max angle range for ped and car traffic light + double max_angle_range; + if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { + max_angle_range = + tier4_autoware_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + } else { + max_angle_range = tier4_autoware_utils::deg2rad(config_.car_traffic_light_max_angle_range); + } // traffic light bottom left const auto & tl_bl = traffic_light.front(); // traffic light bottom right @@ -467,7 +532,6 @@ void MapBasedDetector::getVisibleTrafficLights( // check angle range const double tl_yaw = tier4_autoware_utils::normalizeRadian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); - constexpr double max_angle_range = tier4_autoware_utils::deg2rad(40.0); // get direction of z axis tf2::Vector3 camera_z_dir(0, 0, 1); diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp index 71930c65f88c9..ae6ba8c1feedb 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace traffic_light { @@ -71,7 +72,8 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg, + const uint8_t traffic_light_type); rclcpp::Subscription::SharedPtr map_sub_; /** @@ -89,7 +91,6 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node * */ std::shared_ptr cloud_occlusion_predictor_; - typedef perception_utils::PrimeSynchronizer< tier4_perception_msgs::msg::TrafficSignalArray, tier4_perception_msgs::msg::TrafficLightRoiArray, sensor_msgs::msg::CameraInfo, @@ -97,6 +98,11 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node SynchronizerType; std::shared_ptr synchronizer_; + std::shared_ptr synchronizer_ped_; + + std::vector subscribed_; + std::vector occlusion_ratios_; + tier4_perception_msgs::msg::TrafficSignalArray out_msg_; }; } // namespace traffic_light #endif // TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_ diff --git a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml b/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml index 795267b920e24..aa7e34f8a4a09 100644 --- a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml +++ b/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml @@ -4,7 +4,8 @@ - + + @@ -13,7 +14,8 @@ - + + diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index c460f6a623bc4..6429640ff44c1 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,7 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( // publishers signal_pub_ = create_publisher("~/output/traffic_signals", 1); + // configuration parameters config_.azimuth_occlusion_resolution_deg = declare_parameter("azimuth_occlusion_resolution_deg", 0.15); @@ -72,12 +74,26 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( config_.elevation_occlusion_resolution_deg); const std::vector topics{ - "~/input/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; + "~/input/car/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; const std::vector qos(topics.size(), rclcpp::SensorDataQoS()); synchronizer_ = std::make_shared( this, topics, qos, - std::bind(&TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4), + std::bind( + &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT), + config_.max_image_cloud_delay, config_.max_wait_t); + + const std::vector topics_ped{ + "~/input/pedestrian/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; + const std::vector qos_ped(topics_ped.size(), rclcpp::SensorDataQoS()); + synchronizer_ped_ = std::make_shared( + this, topics_ped, qos_ped, + std::bind( + &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT), config_.max_image_cloud_delay, config_.max_wait_t); + + subscribed_.resize(2, false); } void TrafficLightOcclusionPredictorNodelet::mapCallback( @@ -109,27 +125,65 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg) + const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg, + const uint8_t traffic_light_type) { - tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; std::vector occlusion_ratios; - if ( - in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr || - in_roi_msg->rois.size() != in_signal_msg->signals.size()) { - occlusion_ratios.resize(out_msg.signals.size(), 0); + size_t not_detected_roi = 0; + if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { + occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { - cloud_occlusion_predictor_->predict( - in_cam_info_msg, in_roi_msg, in_cloud_msg, tf_buffer_, traffic_light_position_map_, - occlusion_ratios); + tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; + selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); + for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { + // not detected roi + if (in_roi_msg->rois[i].roi.height == 0) { + not_detected_roi++; + continue; + } + if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) { + selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i)); + } + } + + tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; + + if (selected_roi_msg.rois.size() != in_signal_msg->signals.size() - not_detected_roi) { + occlusion_ratios.resize(in_signal_msg->signals.size(), 0); + } else { + auto selected_roi_msg_ptr = + std::make_shared(selected_roi_msg); + cloud_occlusion_predictor_->predict( + in_cam_info_msg, selected_roi_msg_ptr, in_cloud_msg, tf_buffer_, + traffic_light_position_map_, occlusion_ratios); + } } + + size_t predicted_num = out_msg_.signals.size(); + for (size_t i = 0; i < occlusion_ratios.size(); i++) { + out_msg_.signals.push_back(in_signal_msg->signals.at(i)); + if (occlusion_ratios[i] >= config_.max_occlusion_ratio) { - traffic_light_utils::setSignalUnknown(out_msg.signals[i]); + traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0); } } - signal_pub_->publish(out_msg); -} + // push back not detected rois + for (size_t i = occlusion_ratios.size(); i < in_signal_msg->signals.size(); ++i) { + out_msg_.signals.push_back(in_signal_msg->signals[i]); + } + + subscribed_.at(traffic_light_type) = true; + + if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) { + auto pub_msg = std::make_unique(out_msg_); + pub_msg->header = in_signal_msg->header; + signal_pub_->publish(std::move(pub_msg)); + out_msg_.signals.clear(); + std::fill(subscribed_.begin(), subscribed_.end(), false); + } +} } // namespace traffic_light #include diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 7daa0849abe5e..6f96913420bad 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -88,6 +88,10 @@ void CloudOcclusionPredictor::predict( image_geometry::PinholeCameraModel pinhole_model; pinhole_model.fromCameraInfo(*camera_info_msg); for (size_t i = 0; i < rois_msg->rois.size(); i++) { + // skip if no detection + if (rois_msg->rois[i].roi.height == 0) { + continue; + } calcRoiVector3D( rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i], roi_brs[i]); @@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict( lidar_rays_[static_cast(ray.azimuth)][static_cast(ray.elevation)].push_back(ray); } for (size_t i = 0; i < roi_tls.size(); i++) { - occlusion_ratios[i] = predict(roi_tls[i], roi_brs[i]); + occlusion_ratios[i] = rois_msg->rois[i].roi.height == 0 ? 0 : predict(roi_tls[i], roi_brs[i]); } } diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp similarity index 88% rename from perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp rename to perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp index 3f83cf6e926ad..5e981bbeafda1 100644 --- a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ -#define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include #include @@ -49,4 +49,4 @@ class TrafficLightMapVisualizerNode : public rclcpp::Node } // namespace traffic_light -#endif // TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ diff --git a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp similarity index 95% rename from perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp rename to perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp index 1c64eb2f9e82b..13d9045e89e98 100644 --- a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp @@ -11,8 +11,8 @@ // 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 TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ -#define TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ #include #include @@ -123,4 +123,4 @@ class TrafficLightRoiVisualizerNodelet : public rclcpp::Node } // namespace traffic_light -#endif // TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp index 02f7a67302945..62d416457c6f0 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" + #include -#include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index 53e06e47a1873..e621a20450bbf 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" + #include #include #include -#include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index 2f2d80ba43233..4ff9729695be2 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp" // NOLINT(whitespace/line_length) + #include #include -#include #include #include diff --git a/planning/.pages b/planning/.pages index 7cbd36d2ca2d0..e244f1d1ea25d 100644 --- a/planning/.pages +++ b/planning/.pages @@ -5,18 +5,18 @@ nav: - 'Concept': - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design - - 'Path Generation': planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design - - 'Collision Assessment/Safety Check': planning/behavior_path_planner/docs/behavior_path_planner_safety_check - - 'Dynamic Drivable Area': planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design - - 'Turn Signal': planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design + - 'Path Generation': planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design + - 'Collision Assessment/Safety Check': planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check + - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design + - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design - - 'Avoidance by Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design - - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design - - 'Goal Planner': planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design - - 'Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design - - 'Side Shift': planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design - - 'Start Planner': planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design + - 'Avoidance': planning/behavior_path_avoidance_module + - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module + - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module + - 'Goal Planner': planning/behavior_path_goal_planner_module + - 'Lane Change': planning/behavior_path_lane_change_module + - 'Side Shift': planning/behavior_path_side_shift_module + - 'Start Planner': planning/behavior_path_start_planner_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/behavior_velocity_planner - 'Template for Custom Module': planning/behavior_velocity_template_module diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md b/planning/behavior_path_avoidance_by_lane_change_module/README.md similarity index 86% rename from planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md rename to planning/behavior_path_avoidance_by_lane_change_module/README.md index 2c91cc43995ac..d91d7116ee056 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md +++ b/planning/behavior_path_avoidance_by_lane_change_module/README.md @@ -9,11 +9,11 @@ This module is designed as one of the obstacle avoidance features and generates - Exist lane changeable lanelet. - Exist avoidance target objects on ego driving lane. -![avoidance_by_lane_change](../image/avoidance_by_lane_change/avoidance_by_lane_change.svg) +![avoidance_by_lane_change](./images/avoidance_by_lane_change.svg) ## Inner-workings / Algorithms -Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](./behavior_path_planner_avoidance_design.md) and the path generation logic of the [Normal Lane Change Module](./behavior_path_planner_lane_change_design.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](../behavior_path_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. Check that the following conditions are satisfied after the filtering process for the avoidance target. @@ -21,7 +21,7 @@ Check that the following conditions are satisfied after the filtering process fo This module is launched when the number of avoidance target objects on **EGO DRIVING LANE** is greater than `execute_object_num`. If there are no avoidance targets in the ego driving lane or their number is less than the parameter, the obstacle is avoided by normal avoidance behavior (if the normal avoidance module is registered). -![trigger_1](../image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg) +![trigger_1](./images/avoidance_by_lc_trigger_1.svg) ### Lane change end point condition @@ -29,7 +29,7 @@ Unlike the normal avoidance module, which specifies the shift line end point, th Although setting the parameter to `false` would increase the scene of avoidance by lane change, it is assumed that sufficient lateral margin may not be ensured in some cases because the vehicle passes by the side of obstacles during the lane change. -![trigger_2](../image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg) +![trigger_2](./images/avoidance_by_lc_trigger_2.svg) ## Parameters diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml index 9518185d30d63..3e8907cdccdf6 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml @@ -85,3 +85,11 @@ bicycle: false # [-] motorcycle: false # [-] pedestrian: false # [-] + + constraints: + # lateral constraints + lateral: + velocity: [1.0, 1.38, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg b/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg rename to planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg b/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg rename to planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg b/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg rename to planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp index e177244930da6..8e7d1f67d3157 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -16,8 +16,6 @@ #include "behavior_path_avoidance_module/data_structs.hpp" -#include - namespace behavior_path_planner { struct AvoidanceByLCParameters : public AvoidanceParameters @@ -27,6 +25,10 @@ struct AvoidanceByLCParameters : public AvoidanceParameters // execute only when lane change end point is before the object. bool execute_only_when_lane_change_finish_before_object{false}; + + explicit AvoidanceByLCParameters(const AvoidanceParameters & param) : AvoidanceParameters(param) + { + } }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp index 0bc08ccd400ca..897956a392008 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -17,7 +17,7 @@ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include @@ -40,6 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface bool isExecutionRequested() const override; + void processOnEntry() override; + protected: void updateRTCStatus(const double start_distance, const double finish_distance) override; }; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp index b1beae9c6fc03..d996555365166 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp @@ -17,7 +17,7 @@ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_avoidance_by_lane_change_module/interface.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 1e667c207c64f..7a82ba60a1eaa 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -16,13 +16,15 @@ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include namespace behavior_path_planner { using AvoidanceDebugData = DebugData; +using helper::avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { @@ -46,6 +48,7 @@ class AvoidanceByLaneChange : public NormalLaneChange ObjectDataArray registered_objects_; mutable ObjectDataArray stopped_objects_; + std::shared_ptr avoidance_helper_; ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index da51dd9f235dc..0f9f3f6dc9d42 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -19,8 +19,10 @@ eigen3_cmake_module behavior_path_avoidance_module + behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common + lanelet2_extension motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 2e451461abab7..efdb302a58ac2 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -45,7 +45,11 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { - return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck(); +} +void AvoidanceByLaneChangeInterface::processOnEntry() +{ + waitApproval(); } void AvoidanceByLaneChangeInterface::updateRTCStatus( diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 61b0a36ae0fb0..3f810710ef37b 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -37,7 +39,9 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) // init lane change manager LaneChangeModuleManager::init(node); - AvoidanceByLCParameters p{}; + const auto avoidance_params = getParameter(node); + AvoidanceByLCParameters p(avoidance_params); + // unique parameters { const std::string ns = "avoidance_by_lane_change."; @@ -139,6 +143,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + { const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index bfa873d9518ef..b76a9e5645b48 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -15,8 +15,13 @@ #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" + +#include +#include #include #include @@ -25,11 +30,22 @@ namespace behavior_path_planner { +namespace +{ +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} +} // namespace AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), - avoidance_parameters_(std::move(avoidance_parameters)) + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_helper_{std::make_shared(avoidance_parameters_)} { } @@ -37,48 +53,69 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const { const auto & data = avoidance_data_; - if (!status_.is_safe) { + if (data.target_objects.empty()) { + RCLCPP_DEBUG(logger_, "no empty objects"); return false; } const auto & object_parameters = avoidance_parameters_->object_parameters; - const auto is_more_than_threshold = - std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) { - const auto & objects = avoidance_data_.target_objects; - - const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) { - const auto target_class = - utils::getHighestProbLabel(object.object.classification) == p.first; - return target_class && object.avoid_required; - }); - return num >= p.second.execute_num; - }); + const auto count_target_object = [&](const auto sum, const auto & p) { + const auto & objects = avoidance_data_.target_objects; - if (!is_more_than_threshold) { - return false; - } + const auto is_avoidance_target = [&p](const auto & object) { + const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }; - const auto & front_object = data.target_objects.front(); + return sum + std::count_if(objects.begin(), objects.end(), is_avoidance_target); + }; + const auto num_of_avoidance_targets = + std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object); - const auto THRESHOLD = avoidance_parameters_->execute_object_longitudinal_margin; - if (front_object.longitudinal < THRESHOLD) { + if (num_of_avoidance_targets < 1) { + RCLCPP_DEBUG(logger_, "no avoidance target"); return false; } - const auto path = status_.lane_change_path.path; - const auto to_lc_end = motion_utils::calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.info.shift_line.end.position); - const auto execute_only_when_lane_change_finish_before_object = - avoidance_parameters_->execute_only_when_lane_change_finish_before_object; - const auto not_enough_distance = front_object.longitudinal < to_lc_end; - - if (execute_only_when_lane_change_finish_before_object && not_enough_distance) { + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "no empty lanes"); return false; } - return true; + const auto & nearest_object = data.target_objects.front(); + + // get minimum lane change distance + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, + lane_change_parameters_->backward_length_buffer_for_end_of_lane); + + // get minimum avoid distance + + const auto ego_width = getCommonParam().vehicle_width; + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); + const auto nearest_object_parameter = + avoidance_parameters_->object_parameters.at(nearest_object_type); + const auto avoid_margin = + nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + + nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + + const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); + + RCLCPP_DEBUG( + logger_, + "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " + "%.3f", + nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); + + return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); } bool AvoidanceByLaneChange::specialExpiredCheck() const @@ -125,6 +162,23 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( data.current_lanelets = getCurrentLanes(); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, avoidance_parameters_, false)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, + avoidance_parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, + avoidance_parameters_->use_intersection_areas, false)); + // get related objects from dynamic_objects, and then separates them as target objects and non // target objects fillAvoidanceTargetObjects(data, debug); @@ -182,6 +236,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( using boost::geometry::return_centroid; using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcLateralDeviation; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -213,8 +268,11 @@ ObjectData AvoidanceByLaneChange::createObjectData( utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); // Calc lateral deviation from path to target object. - object_data.lateral = - tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + object_data.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 2264669fdcdbe..581aafa6c860c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include @@ -47,6 +48,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); @@ -63,8 +66,7 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_planner") + - "/config/lane_change/lane_change.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + "/config/avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_avoidance_module/README.md similarity index 97% rename from planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md rename to planning/behavior_path_avoidance_module/README.md index c4580bb1e82c3..20c0985f0f333 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -20,7 +20,7 @@ This module executes avoidance over lanes, and the decision requires the lane st The following figure shows a simple explanation of the logic for avoidance path generation. First, target objects are picked up, and shift requests are generated for each object. These shift requests are generated by taking into account the lateral jerk required for avoidance (red lines). Then these requests are merged and the shift points are created on the reference path (blue line). Filtering operations are performed on the shift points such as removing unnecessary shift points (yellow line), and finally a smooth avoidance path is generated by combining Clothoid-like curve primitives (green line). -![fig1](../image/avoidance/avoidance_design.fig1.drawio.svg) +![fig1](./images/avoidance_design.fig1.drawio.svg) ### Flowchart @@ -185,7 +185,7 @@ The avoidance target should be limited to stationary objects (you should not avo - This means that the vehicle is parked on the edge of the lane. This prevents the vehicle from avoiding a vehicle waiting at a traffic light in the middle of the lane. However, this is not an appropriate implementation for the purpose. Even if a vehicle is in the center of the lane, it should be avoided if it has its hazard lights on, and this is a point that should be improved in the future as the recognition performance improves. - Object is not behind ego(default: > -`2.0 m`) or too far(default: < `150.0 m`) and object is not behind the path goal. -![fig1](../image/avoidance/target_vehicle_selection.svg) +![fig1](./images/target_vehicle_selection.svg) ### Parked-car detection @@ -203,7 +203,7 @@ $$ The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually. -![fig2](../image/avoidance/parked-car-detection.svg) +![fig2](./images/parked-car-detection.svg) ### Compensation for detection lost @@ -365,9 +365,9 @@ Therefore, in order to reduce the influence of the noise, avoidance module gener object_envelope_buffer: 0.3 # [m] ``` -![fig1](../image/avoidance/envelope_polygon.svg) +![fig1](./images/envelope_polygon.svg) -![fig1](../image/avoidance/shift_line_generation.svg) +![fig1](./images/shift_line_generation.svg) ### Computing Shift Length and Shift Points @@ -388,7 +388,7 @@ else The following figure illustrates these variables(This figure just shows the max value of lateral shift length). -![shift_point_and_its_constraints](../image/avoidance/avoidance_module-shift_point_and_its_constraints.drawio.png) +![shift_point_and_its_constraints](./images/avoidance_module-shift_point_and_its_constraints.drawio.png) ### Rationale of having safety buffer and safety margin @@ -399,7 +399,7 @@ To compute the shift length, additional parameters that can be tune are `lateral - It is recommended to set the value to more than half of the ego vehicle's width. - The `road_shoulder_safety_margin` will prevent the module from generating a path that might cause the vehicle to go too near the road shoulder or adjacent lane dividing line. -![shift_length_parameters](../image/shift_length_parameters.drawio.svg) +![shift_length_parameters](./images/shift_length_parameters.drawio.svg) ### Generating path only within lanelet boundaries @@ -410,7 +410,7 @@ The shift length is set as a constant value before the feature is implemented. S These elements are used to compute the distance from the object to the road's shoulder (`to_road_shoulder_distance`). The parameters `use_adjacent_lane` and `use_opposite_lane` allows further configuration of the to `to_road_shoulder_distance`. The following image illustrates the configuration. -![obstacle_to_road_shoulder_distance](../image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg) +![obstacle_to_road_shoulder_distance](./images/obstacle_to_road_shoulder_distance.drawio.svg) If one of the following conditions is `false`, then the shift point will not be generated. @@ -565,7 +565,7 @@ To solve that, this module provides a parameter for the minimum avoidance speed, - If the ego vehicle speed is lower than "nominal" minimum speed, use the minimum speed in the calculation of the jerk. - If the ego vehicle speed is lower than "sharp" minimum speed and a nominal lateral jerk is not enough for avoidance (the case where the ego vehicle is stopped close to the obstacle), use the "sharp" minimum speed in the calculation of the jerk (it should be lower than "nominal" speed). -![fig1](../image/avoidance/how_to_decide_path_shape_one_object.drawio.svg) +![fig1](./images/how_to_decide_path_shape_one_object.drawio.svg) #### Multiple obstacle case (one direction) @@ -575,13 +575,13 @@ Generate shift points for multiple obstacles. All of them are merged to generate For the details of the shift point filtering, see [filtering for shift points](#filtering-for-shift-points). -![fig1](../image/avoidance/how_to_decide_path_shape_multi_object_one_direction.drawio.svg) +![fig1](./images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg) #### Multiple obstacle case (both direction) Generate shift points for multiple obstacles. All of them are merged to generate new shift points. If there are areas where the desired shifts conflict in different directions, the sum of the maximum shift amounts of these areas is used as the final shift amount. The rest of the process is the same as in the case of one direction. -![fig1](../image/avoidance/how_to_decide_path_shape_multi_object_both_direction.drawio.svg) +![fig1](./images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg) #### Filtering for shift points @@ -608,23 +608,23 @@ use_hatched_road_markings: false #### adjacent lane -![fig1](../image/avoidance/use_adjacent_lane.svg) +![fig1](./images/use_adjacent_lane.svg) #### opposite lane -![fig1](../image/avoidance/use_opposite_lane.svg) +![fig1](./images/use_opposite_lane.svg) #### intersection areas The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) -![fig1](../image/avoidance/use_intersection_areas.svg) +![fig1](./images/use_intersection_areas.svg) #### hatched road markings The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) -![fig1](../image/avoidance/use_hatched_road_markings.svg) +![fig1](./images/use_hatched_road_markings.svg) ### Safety check @@ -647,11 +647,11 @@ safety_check_backward_distance: 50.0 # [m] safety_check_accel_for_rss: 2.5 # [m/ss] ``` -![fig1](../image/avoidance/safety_check_flowchart.svg) +![fig1](./images/safety_check_flowchart.svg) `safety_check_backward_distance` is the parameter related to the safety check area. The module checks a collision risk for all vehicle that is within shift side lane and between object `object_check_forward_distance` ahead and `safety_check_backward_distance` behind. -![fig1](../image/avoidance/safety_check_step_1.svg) +![fig1](./images/safety_check_step_1.svg) **NOTE**: Even if a part of an object polygon overlaps the detection area, if the center of gravity of the object does not exist on the lane, the vehicle is excluded from the safety check target. @@ -659,11 +659,11 @@ safety_check_accel_for_rss: 2.5 # [m/ss] Judge the risk of collision based on ego future position and object prediction path. The module calculates Ego's future position in the time horizon (`safety_check_time_horizon`), and use object's prediction path as object future position. -![fig1](../image/avoidance/safety_check_step_2.svg) +![fig1](./images/safety_check_step_2.svg) After calculating the future position of Ego and object, the module calculates the lateral/longitudinal deviation of Ego and the object. The module also calculates the lateral/longitudinal margin necessary to determine that it is safe to execute avoidance maneuver, and if both the lateral and longitudinal distances are less than the margins, it determines that there is a risk of a collision at that time. -![fig1](../image/avoidance/safety_check_margin.svg) +![fig1](./images/safety_check_margin.svg) The value of the longitudinal margin is calculated based on Responsibility-Sensitive Safety theory ([RSS](https://newsroom.intel.com/articles/rss-explained-five-rules-autonomous-vehicle-safety/#gs.ljzofv)). The `safety_check_idling_time` represents $T_{idle}$, and `safety_check_accel_for_rss` represents $a_{max}$. @@ -673,7 +673,7 @@ $$ The lateral margin is changeable based on ego longitudinal velocity. If the vehicle is driving at a high speed, the lateral margin should be larger, and if the vehicle is driving at a low speed, the value of the lateral margin should be set to a smaller value. Thus, the lateral margin for each vehicle speed is set as a parameter, and the module determines the lateral margin from the current vehicle speed as shown in the following figure. -![fig1](../image/avoidance/dynamic_lateral_margin.svg) +![fig1](./images/dynamic_lateral_margin.svg) ```yaml target_velocity_matrix: @@ -695,7 +695,7 @@ If an avoidance path can be generated and it is determined that avoidance maneuv yield_velocity: 2.78 # [m/s] ``` -![fig1](../image/avoidance/yield_maneuver.svg) +![fig1](./images/yield_maneuver.svg) **NOTE**: In yield maneuver, the vehicle decelerates target velocity under constraints. @@ -736,9 +736,9 @@ $$ SHIFT_{current} > L_{threshold} $$ -![fig1](../image/avoidance/yield_limitation.svg) +![fig1](./images/yield_limitation.svg) -![fig1](../image/avoidance/yield_maneuver_flowchart.svg) +![fig1](./images/yield_maneuver_flowchart.svg) --- @@ -935,7 +935,7 @@ namespace: `avoidance.constraints.longitudinal.` - **Planning on the intersection** - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop.　 This is especially important at intersections. -![fig1](../image/avoidance/intersection_problem.drawio.svg) +![fig1](./images/intersection_problem.drawio.svg) - **Safety Check** @@ -963,7 +963,7 @@ namespace: `avoidance.constraints.longitudinal.` Developers can see what is going on in each process by visualizing all the avoidance planning process outputs. The example includes target vehicles, shift points for each object, shift points after each filtering process, etc. -![fig1](../image/avoidance/avoidance-debug-marker.png) +![fig1](./images/avoidance-debug-marker.png) To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. @@ -971,7 +971,7 @@ To enable the debug marker, execute `ros2 param set /planning/scenario_planning/ If for some reason, no shift point is generated for your object, you can check for the failure reason via `ros2 topic echo`. -![avoidance_debug_message_array](../image/avoidance/avoidance_debug_message_array.png) +![avoidance_debug_message_array](./images/avoidance_debug_message_array.png) To print the debug message, just run the following diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index c68c10f2a9489..2d38cebd591f5 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -141,6 +141,7 @@ force_avoidance: enable: false # [-] time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.0 # [m] diff --git a/planning/behavior_path_planner/image/avoidance/avoidance-debug-marker.png b/planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance-debug-marker.png rename to planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png diff --git a/planning/behavior_path_planner/image/avoidance/avoidance_debug_message_array.png b/planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance_debug_message_array.png rename to planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png diff --git a/planning/behavior_path_planner/image/avoidance/avoidance_design.fig1.drawio.svg b/planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance_design.fig1.drawio.svg rename to planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/avoidance_module-shift_point_and_its_constraints.drawio.png b/planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance_module-shift_point_and_its_constraints.drawio.png rename to planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png diff --git a/planning/behavior_path_planner/image/avoidance/dynamic_lateral_margin.svg b/planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/dynamic_lateral_margin.svg rename to planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg diff --git a/planning/behavior_path_planner/image/avoidance/envelope_polygon.svg b/planning/behavior_path_avoidance_module/images/envelope_polygon.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/envelope_polygon.svg rename to planning/behavior_path_avoidance_module/images/envelope_polygon.svg diff --git a/planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_both_direction.drawio.svg b/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_both_direction.drawio.svg rename to planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_one_direction.drawio.svg b/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_one_direction.drawio.svg rename to planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_one_object.drawio.svg b/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_one_object.drawio.svg rename to planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/intersection_problem.drawio.svg b/planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/intersection_problem.drawio.svg rename to planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg b/planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg rename to planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/parked-car-detection.svg b/planning/behavior_path_avoidance_module/images/parked-car-detection.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/parked-car-detection.svg rename to planning/behavior_path_avoidance_module/images/parked-car-detection.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_flowchart.svg b/planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_flowchart.svg rename to planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_margin.svg b/planning/behavior_path_avoidance_module/images/safety_check_margin.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_margin.svg rename to planning/behavior_path_avoidance_module/images/safety_check_margin.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_step_1.svg b/planning/behavior_path_avoidance_module/images/safety_check_step_1.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_step_1.svg rename to planning/behavior_path_avoidance_module/images/safety_check_step_1.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_step_2.svg b/planning/behavior_path_avoidance_module/images/safety_check_step_2.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_step_2.svg rename to planning/behavior_path_avoidance_module/images/safety_check_step_2.svg diff --git a/planning/behavior_path_planner/image/shift_length_parameters.drawio.svg b/planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/shift_length_parameters.drawio.svg rename to planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/shift_line_generation.svg b/planning/behavior_path_avoidance_module/images/shift_line_generation.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/shift_line_generation.svg rename to planning/behavior_path_avoidance_module/images/shift_line_generation.svg diff --git a/planning/behavior_path_planner/image/avoidance/target_vehicle_selection.svg b/planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/target_vehicle_selection.svg rename to planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg diff --git a/planning/behavior_path_planner/image/avoidance/todo.png b/planning/behavior_path_avoidance_module/images/todo.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/todo.png rename to planning/behavior_path_avoidance_module/images/todo.png diff --git a/planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg b/planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg rename to planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg diff --git a/planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg b/planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg rename to planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg diff --git a/planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg b/planning/behavior_path_avoidance_module/images/use_intersection_areas.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg rename to planning/behavior_path_avoidance_module/images/use_intersection_areas.svg diff --git a/planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg b/planning/behavior_path_avoidance_module/images/use_opposite_lane.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg rename to planning/behavior_path_avoidance_module/images/use_opposite_lane.svg diff --git a/planning/behavior_path_planner/image/avoidance/yield_limitation.svg b/planning/behavior_path_avoidance_module/images/yield_limitation.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/yield_limitation.svg rename to planning/behavior_path_avoidance_module/images/yield_limitation.svg diff --git a/planning/behavior_path_planner/image/avoidance/yield_maneuver.svg b/planning/behavior_path_avoidance_module/images/yield_maneuver.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/yield_maneuver.svg rename to planning/behavior_path_avoidance_module/images/yield_maneuver.svg diff --git a/planning/behavior_path_planner/image/avoidance/yield_maneuver_flowchart.svg b/planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/yield_maneuver_flowchart.svg rename to planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 44c8dedec236d..1d1494b7719c3 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -19,12 +19,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include #include -#include #include #include @@ -34,6 +33,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -48,10 +48,11 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::TransformStamped; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using route_handler::Direction; + struct ObjectParameter { bool is_avoidance_target{false}; @@ -175,6 +176,7 @@ struct AvoidanceParameters // force avoidance double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; + double force_avoidance_distance_threshold{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction @@ -326,24 +328,38 @@ struct AvoidanceParameters struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) - : object(obj), lateral(lat), longitudinal(lon), length(len), overhang_dist(overhang) + + ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang) + : object(std::move(obj)), + to_centerline(lat), + longitudinal(lon), + length(len), + overhang_dist(overhang) { } PredictedObject object; + // object behavior. + enum class Behavior { + NONE = 0, + MERGING, + DEVIATING, + }; + Behavior behavior{Behavior::NONE}; + // lateral position of the CoM, in Frenet coordinate from ego-pose - double lateral; + + double to_centerline{0.0}; // longitudinal position of the CoM, in Frenet coordinate from ego-pose - double longitudinal; + double longitudinal{0.0}; // longitudinal length of vehicle, in Frenet coordinate - double length; + double length{0.0}; // lateral distance to the closest footprint, in Frenet coordinate - double overhang_dist; + double overhang_dist{0.0}; // lateral shiftable ratio double shiftable_ratio{0.0}; @@ -366,6 +382,9 @@ struct ObjectData // avoidance target // store the information of the lanelet which the object's overhang is currently occupying lanelet::ConstLanelet overhang_lanelet; + // the position at the detected moment + Pose init_pose; + // the position of the overhang Pose overhang_pose; @@ -396,11 +415,17 @@ struct ObjectData // avoidance target // is within intersection area bool is_within_intersection{false}; + // object direction. + Direction direction{Direction::NONE}; + // unavoidable reason - std::string reason{""}; + std::string reason{}; // lateral avoid margin std::optional avoid_margin{std::nullopt}; + + // the nearest bound point (use in road shoulder distance calculation) + std::optional nearest_bound_point{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -437,8 +462,8 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) - : avoid_line{avoid_line}, return_line{return_line} + AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } @@ -563,8 +588,6 @@ struct ShiftLineData */ struct DebugData { - std::shared_ptr current_lanelets; - geometry_msgs::msg::Polygon detection_area; lanelet::ConstLineStrings3d bounds; @@ -615,6 +638,9 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // safety check area + lanelet::ConstLanelets safety_check_lanes; + // collision check debug map utils::path_safety_checker::CollisionCheckDebugMap collision_check; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 2bf3158da9ab6..f49f457ddd066 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -16,33 +16,23 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include -#include #include -#include #include -#include - #include -#include namespace marker_utils::avoidance_marker { -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; -using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; @@ -50,7 +40,7 @@ MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); MarkerArray createAvoidLineMarkerArray( - const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, + const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w); MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); @@ -59,22 +49,15 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray makeOverhangToRoadShoulderMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); - -MarkerArray createOverhangFurthestLineStringMarkerArray( - const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, - const float & g, const float & b); - MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr); +std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr); -std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); +std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 6717cefefc9f6..f5b797978b744 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -58,7 +58,9 @@ class AvoidanceHelper double getEgoSpeed() const { return std::abs(data_->self_odometry->twist.twist.linear.x); } - size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const + geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } + + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( values.begin(), values.end(), [&](const auto value) { return velocity < value; }); @@ -185,10 +187,20 @@ class AvoidanceHelper return parameters_->object_check_max_forward_distance; } + const auto & route_handler = data_->route_handler; + + lanelet::ConstLanelet closest_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &closest_lane)) { + return parameters_->object_check_max_forward_distance; + } + + const auto limit = route_handler->getTrafficRulesPtr()->speedLimit(closest_lane); + const auto speed = isShifted() ? limit.speedLimit.value() : getEgoSpeed(); + const auto max_shift_length = std::max( std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); - const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( - max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + const auto dynamic_distance = + PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed); return std::clamp( 1.5 * dynamic_distance + getNominalPrepareDistance(), diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 72cfbe6f0a1ed..e23a8a96ee7da 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -19,11 +19,10 @@ #include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include +#include +#include #include -#include -#include #include namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp new file mode 100644 index 0000000000000..b9af50ce76eb5 --- /dev/null +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -0,0 +1,368 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ + +#include "tier4_autoware_utils/ros/parameter.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using tier4_autoware_utils::getOrDeclareParameter; + +AvoidanceParameters getParameter(rclcpp::Node * node) +{ + AvoidanceParameters p{}; + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); + p.enable_yield_maneuver_during_shifting = + getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); + p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); + } + + // drivable area + { + const std::string ns = "avoidance."; + p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); + p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); + p.use_hatched_road_markings = + getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.safety_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.use_conservative_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); + return param; + }; + + const std::string ns = "avoidance.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_check_yaw_deviation = + getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + const std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.force_avoidance_distance_threshold = + getOrDeclareParameter(*node, ns + "distance_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + + // safety check general params + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_safety_check_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.safety_check."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); + p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); + p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); + p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); + p.check_unavoidable_object = + getOrDeclareParameter(*node, ns + "check_unavoidable_object"); + p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); + p.check_all_predicted_path = + getOrDeclareParameter(*node, ns + "check_all_predicted_path"); + p.safety_check_backward_distance = + getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = + getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + } + + // safety check predicted path params + { + const 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_front_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); + 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 + { + const std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + getOrDeclareParameter(*node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); + } + + // avoidance maneuver (lateral) + { + const std::string ns = "avoidance.avoidance.lateral."; + p.soft_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); + p.lateral_execution_threshold = + getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); + p.lateral_small_shift_threshold = + getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); + p.lateral_avoid_check_threshold = + getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); + p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + } + + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + + // avoidance maneuver (return shift dead line) + { + const std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + + // yield + { + const std::string ns = "avoidance.yield."; + p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); + } + + // stop + { + const std::string ns = "avoidance.stop."; + p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); + p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); + } + + // policy + { + const std::string ns = "avoidance.policy."; + p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); + p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); + p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); + p.use_shorten_margin_immediately = + getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + + if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + } + + // constraints (longitudinal) + { + const std::string ns = "avoidance.constraints.longitudinal."; + p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); + p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); + p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); + p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); + p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + } + + // constraints (lateral) + { + const std::string ns = "avoidance.constraints.lateral."; + p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); + p.lateral_max_accel_map = + getOrDeclareParameter>(*node, ns + "max_accel_values"); + p.lateral_min_jerk_map = + getOrDeclareParameter>(*node, ns + "min_jerk_values"); + p.lateral_max_jerk_map = + getOrDeclareParameter>(*node, ns + "max_jerk_values"); + + if (p.velocity_map.empty()) { + throw std::domain_error("invalid velocity map."); + } + + if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + } + + // shift line pipeline + { + const std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = + getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); + } + return p; +} +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 882806177b4e0..889d48c481b6f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -20,9 +20,10 @@ #include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include +#include +#include #include #include @@ -30,11 +31,9 @@ #include #include -#include #include #include #include -#include #include namespace behavior_path_planner @@ -326,13 +325,6 @@ class AvoidanceModule : public SceneModuleInterface // generate output data - /** - * @brief calculate turn signal infomation. - * @param avoidance path. - * @return turn signal command. - */ - TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - /** * @brief fill debug markers. */ @@ -374,8 +366,8 @@ class AvoidanceModule : public SceneModuleInterface */ void removeRegisteredShiftLines() { - constexpr double THRESHOLD = 0.1; - if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + constexpr double threshold = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > threshold) { RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset registered shift lines."); return; } @@ -396,7 +388,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief remove behind shift lines. * @param path shifter. */ - void postProcess() + void postProcess() override { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); @@ -411,8 +403,6 @@ class AvoidanceModule : public SceneModuleInterface using RegisteredShiftLineArray = std::vector; - bool is_avoidance_maneuver_starts; - bool arrived_path_end_{false}; bool safe_{true}; @@ -433,14 +423,20 @@ class AvoidanceModule : public SceneModuleInterface UUID candidate_uuid_; + // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; - mutable size_t safe_count_{0}; + // TODO(Satoshi OTA) remove mutable. + mutable ObjectDataArray detected_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray ego_stopped_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray stopped_objects_; + mutable size_t safe_count_{0}; + mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index edfe9ab9663ce..efdda622a664c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -17,11 +17,8 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include - #include namespace behavior_path_planner::utils::avoidance @@ -29,8 +26,6 @@ namespace behavior_path_planner::utils::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::helper::avoidance::AvoidanceHelper; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; class ShiftLineGenerator { diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index c954ab9022853..dc602edcc8b86 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -117,6 +117,8 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects); + void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); @@ -135,13 +137,6 @@ double getRoadShoulderDistance( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -double extendToRoadShoulderDistanceWithPolygon( - const std::shared_ptr & rh, - const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, - const bool use_intersection_areas); - void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); @@ -158,7 +153,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift); + const std::shared_ptr & parameters, const bool is_right_shift, + DebugData & debug); std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, @@ -167,7 +163,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters, const bool in_avoidance_maneuver); double calcDistanceToReturnDeadLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, @@ -178,6 +174,10 @@ double calcDistanceToAvoidStartLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + +TurnSignalInfo calcTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, + const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml index 642f83497acfb..f72adf0591493 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/behavior_path_avoidance_module/package.xml @@ -27,7 +27,6 @@ behavior_path_planner_common geometry_msgs lanelet2_extension - libboost-dev magic_enum motion_utils objects_of_interest_marker_interface diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 3ae210e0b1d78..ee97efe37d440 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -14,16 +14,15 @@ #include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include -#include -#include - #include #include @@ -109,6 +108,42 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: return msg; } +MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + for (const auto & object : objects) { + if (!object.nearest_bound_point.has_value()) { + continue; + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999)); + + marker.points.push_back(object.overhang_pose.position); + marker.points.push_back(object.nearest_bound_point.value()); + marker.id = uuidToInt32(object.object.object_id); + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.pose.position = object.nearest_bound_point.value(); + std::ostringstream string_stream; + string_stream << object.to_road_shoulder_distance << "[m]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + } + + return msg; +} + MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; @@ -124,7 +159,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral:" << object.lateral << " [m]\n" + << "lateral:" << object.to_centerline << " [m]\n" << "necessity:" << object.avoid_required << " [-]\n" << "stoppable:" << object.is_stoppable << " [-]\n" << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" @@ -166,6 +201,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); return msg; } @@ -183,6 +219,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); return msg; } @@ -418,58 +455,37 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray makeOverhangToRoadShoulderMarkerArray( - const ObjectDataArray & objects, std::string && ns) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0)); - - int32_t i{0}; - MarkerArray msg; - for (const auto & object : objects) { - marker.id = ++i; - marker.pose = object.overhang_pose; - std::ostringstream string_stream; - string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])"; - marker.text = string_stream.str(); - msg.markers.push_back(marker); - } - - return msg; -} - -MarkerArray createOverhangFurthestLineStringMarkerArray( - const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, - const float & g, const float & b) +MarkerArray createDrivableBounds( + const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, + const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); MarkerArray msg; - for (const auto & linestring : linestrings) { - const auto id = static_cast(linestring.id()); + // right bound + { auto marker = createDefaultMarker( - "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), + "map", current_time, ns + "_right", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - for (const auto & p : linestring.basicLineString()) { + for (const auto & p : data.right_bound) { marker.points.push_back(createPoint(p.x(), p.y(), p.z())); } + msg.markers.push_back(marker); + } + + // left bound + { + auto marker = createDefaultMarker( + "map", current_time, ns + "_left", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), + createMarkerColor(r, g, b, 0.999)); + + for (const auto & p : data.left_bound) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } - Marker marker_linestring_id = createDefaultMarker( - "map", current_time, "linestring_id", id, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.8)); - Pose text_id_pose; - text_id_pose.position.x = linestring.front().x(); - text_id_pose.position.y = linestring.front().y(); - text_id_pose.position.z = linestring.front().z(); - marker_linestring_id.pose = text_id_pose; - std::ostringstream ss; - ss << "(ID : " << id << ") "; - marker_linestring_id.text = ss.str(); - msg.markers.push_back(marker_linestring_id); + msg.markers.push_back(marker); } return msg; @@ -478,6 +494,8 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) { + using behavior_path_planner::utils::transformToLanelets; + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; @@ -537,6 +555,8 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); addObjects(data.other_objects, std::string("TooNearToGoal")); + addObjects(data.other_objects, std::string("ParallelToEgoLane")); + addObjects(data.other_objects, std::string("MergingToEgoLane")); } // shift line pre-process @@ -591,10 +611,15 @@ MarkerArray createDebugMarkerArray( // misc { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + add(laneletsAsTriangleMarkerArray( + "drivable_lanes", transformToLanelets(data.drivable_lanes), + createMarkerColor(0.16, 1.0, 0.69, 0.2))); + add(laneletsAsTriangleMarkerArray( + "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2))); + add(laneletsAsTriangleMarkerArray( + "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } return msg; diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index f4997d2af12f1..68cee1aa2b523 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "behavior_path_avoidance_module/manager.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -33,332 +34,7 @@ void AvoidanceModuleManager::init(rclcpp::Node * node) // init manager interface initInterface(node, {"left", "right"}); - AvoidanceParameters p{}; - // general params - { - const std::string ns = "avoidance."; - p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); - p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); - p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); - } - - // drivable area - { - const std::string ns = "avoidance."; - p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); - p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); - p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); - p.use_hatched_road_markings = - getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); - param.safety_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); - param.use_conservative_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); - return param; - }; - - const std::string ns = "avoidance.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.target_filtering."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_check_yaw_deviation = - getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); - } - - { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); - } - - { - const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); - p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); - p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); - } - - // safety check general params - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_safety_check_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.safety_check."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); - p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); - p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); - p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); - p.check_unavoidable_object = - getOrDeclareParameter(*node, ns + "check_unavoidable_object"); - p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); - p.check_all_predicted_path = - getOrDeclareParameter(*node, ns + "check_all_predicted_path"); - p.safety_check_backward_distance = - getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); - p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); - p.hysteresis_factor_safe_count = - getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); - } - - // safety check predicted path params - { - const 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 - { - const std::string ns = "avoidance.safety_check."; - p.rss_params.longitudinal_distance_min_threshold = - getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); - p.rss_params.longitudinal_velocity_delta_time = - getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_front_deceleration"); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); - } - - // avoidance maneuver (lateral) - { - const std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = - getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); - p.lateral_small_shift_threshold = - getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); - p.lateral_avoid_check_threshold = - getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); - p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); - p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); - p.max_deviation_from_lane = - getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); - } - - // avoidance maneuver (longitudinal) - { - const std::string ns = "avoidance.avoidance.longitudinal."; - p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); - p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); - p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); - p.nominal_avoidance_speed = - getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); - } - - // avoidance maneuver (return shift dead line) - { - const std::string ns = "avoidance.avoidance.return_dead_line."; - p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); - p.enable_dead_line_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.enable"); - p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); - p.dead_line_buffer_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.buffer"); - } - - // yield - { - const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); - } - - // stop - { - const std::string ns = "avoidance.stop."; - p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); - p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); - } - - // policy - { - const std::string ns = "avoidance.policy."; - p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); - p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); - p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); - p.use_shorten_margin_immediately = - getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); - - if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { - throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); - } - - if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { - throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); - } - } - - // constraints (longitudinal) - { - const std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); - p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); - p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); - p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); - p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); - } - - // constraints (lateral) - { - const std::string ns = "avoidance.constraints.lateral."; - p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); - p.lateral_max_accel_map = - getOrDeclareParameter>(*node, ns + "max_accel_values"); - p.lateral_min_jerk_map = - getOrDeclareParameter>(*node, ns + "min_jerk_values"); - p.lateral_max_jerk_map = - getOrDeclareParameter>(*node, ns + "max_jerk_values"); - - if (p.velocity_map.empty()) { - throw std::domain_error("invalid velocity map."); - } - - if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - - if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - - if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - } - - // shift line pipeline - { - const std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); - p.same_grad_filter_1_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); - p.same_grad_filter_2_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); - p.same_grad_filter_3_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); - p.sharp_shift_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); - } + auto p = getParameter(node); parameters_ = std::make_shared(p); } diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 46c3bbe55d69d..61941eeee6f00 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -17,9 +17,9 @@ #include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -28,14 +28,11 @@ #include #include #include -#include -#include -#include -#include +#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include -#include #include #include @@ -220,21 +217,23 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // lanelet info data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_); + getPreviousModuleOutput().reference_path, planner_data_); data.extend_lanelets = utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes + const auto has_shift_point = !path_shifter_.getShiftLines().empty(); + const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, parameters_, in_avoidance_maneuver)); }); // calc drivable bound - const auto shorten_lanes = - utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + auto tmp_path = getPreviousModuleOutput().path; + const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, true)); @@ -244,9 +243,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { - data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + data.reference_path_rough = extendBackwardLength(getPreviousModuleOutput().path); } else { - data.reference_path_rough = *getPreviousModuleOutput().path; + data.reference_path_rough = getPreviousModuleOutput().path; RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path."); } @@ -328,14 +327,12 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // debug { - debug.current_lanelets = std::make_shared(data.current_lanelets); - std::vector debug_info_array; const auto append = [&](const auto & o) { AvoidanceDebugMsg debug_info; debug_info.object_id = toHexString(o.object.object_id); debug_info.longitudinal_distance = o.longitudinal; - debug_info.lateral_distance_from_centerline = o.lateral; + debug_info.lateral_distance_from_centerline = o.to_centerline; debug_info.allow_avoidance = o.reason == ""; debug_info.failed_reason = o.reason; debug_info_array.push_back(debug_info); @@ -379,8 +376,15 @@ ObjectData AvoidanceModule::createObjectData( // Calc moving time. utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); + // Fill init pose. + utils::avoidance::fillInitialPose(object_data, detected_objects_); + // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); + object_data.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( @@ -735,7 +739,7 @@ bool AvoidanceModule::isSafePath( const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, is_right_shift.value()); + avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); @@ -871,7 +875,7 @@ BehaviorModuleOutput AvoidanceModule::plan() if (success_spline_path_generation && success_linear_path_generation) { helper_->setPreviousLinearShiftPath(linear_shift_path); helper_->setPreviousSplineShiftPath(spline_shift_path); - helper_->setPreviousReferencePath(data.reference_path); + helper_->setPreviousReferencePath(path_shifter_.getReferencePath()); } else { spline_shift_path = helper_->getPreviousSplineShiftPath(); } @@ -884,9 +888,13 @@ BehaviorModuleOutput AvoidanceModule::plan() BehaviorModuleOutput output; // turn signal info - { + if (path_shifter_.getShiftLines().empty()) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(spline_shift_path); + const auto new_signal = utils::avoidance::calcTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, + planner_data_); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -910,17 +918,17 @@ BehaviorModuleOutput AvoidanceModule::plan() } if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { - output.path = std::make_shared(spline_shift_path.path); + output.path = spline_shift_path.path; } else { output.path = getPreviousModuleOutput().path; RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing."); } output.reference_path = getPreviousModuleOutput().reference_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - const size_t ego_idx = planner_data_->findEgoIndex(output.path->points); - utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); + const size_t ego_idx = planner_data_->findEgoIndex(output.path.points); + utils::clipPathLength(output.path, ego_idx, planner_data_->parameters); // Drivable area generation. { @@ -995,7 +1003,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() } path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return out; } @@ -1169,11 +1177,11 @@ void AvoidanceModule::updateData() helper_->setData(planner_data_); if (!helper_->isInitialized()) { - helper_->setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_->setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_->setPreviousReferencePath(*getPreviousModuleOutput().path); + helper_->setPreviousSplineShiftPath(toShiftedPath(getPreviousModuleOutput().path)); + helper_->setPreviousLinearShiftPath(toShiftedPath(getPreviousModuleOutput().path)); + helper_->setPreviousReferencePath(getPreviousModuleOutput().path); helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_)); + getPreviousModuleOutput().reference_path, planner_data_)); } debug_data_ = DebugData(); @@ -1205,6 +1213,12 @@ void AvoidanceModule::updateData() // update rtc status. updateRTCData(); + // update interest objects data + for (const auto & [uuid, data] : debug_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + safe_ = avoid_data_.safe; } @@ -1229,7 +1243,6 @@ void AvoidanceModule::initVariables() debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } @@ -1266,8 +1279,8 @@ void AvoidanceModule::updateRTCData() CandidateOutput output; - const auto sl_front = candidates.front(); - const auto sl_back = candidates.back(); + const auto & sl_front = candidates.front(); + const auto & sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; output.lateral_shift = helper_->getRelativeShiftToPath(shift_line); @@ -1277,118 +1290,6 @@ void AvoidanceModule::updateRTCData() updateCandidateRTCStatus(output); } -TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const -{ - const auto shift_lines = path_shifter_.getShiftLines(); - if (shift_lines.empty()) { - return {}; - } - - const auto front_shift_line = shift_lines.front(); - const size_t start_idx = front_shift_line.start_idx; - const size_t end_idx = front_shift_line.end_idx; - - const auto current_shift_length = helper_->getEgoShift(); - const double start_shift_length = path.shift_length.at(start_idx); - const double end_shift_length = path.shift_length.at(end_idx); - const double segment_shift_length = end_shift_length - start_shift_length; - - const double turn_signal_shift_length_threshold = - planner_data_->parameters.turn_signal_shift_length_threshold; - const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - const double turn_signal_minimum_search_distance = - planner_data_->parameters.turn_signal_minimum_search_distance; - - // If shift length is shorter than the threshold, it does not need to turn on blinkers - if (std::fabs(segment_shift_length) < turn_signal_shift_length_threshold) { - return {}; - } - - // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < 0.1) { - return {}; - } - - // compute blinker start idx and end idx - size_t blinker_start_idx = [&]() { - for (size_t idx = start_idx; idx <= end_idx; ++idx) { - const double current_shift_length = path.shift_length.at(idx); - if (current_shift_length > 0.1) { - return idx; - } - } - return start_idx; - }(); - size_t blinker_end_idx = end_idx; - - // prevent invalid access for out-of-range - blinker_start_idx = - std::min(std::max(std::size_t(0), blinker_start_idx), path.path.points.size() - 1); - blinker_end_idx = - std::min(std::max(blinker_start_idx, blinker_end_idx), path.path.points.size() - 1); - - const auto blinker_start_pose = path.path.points.at(blinker_start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(blinker_end_idx).point.pose; - - const double ego_vehicle_offset = - planner_data_->parameters.vehicle_info.max_longitudinal_offset_m; - const auto signal_prepare_distance = - std::max(getEgoSpeed() * turn_signal_search_time, turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, getEgoPosition(), blinker_start_pose.position) - - ego_vehicle_offset; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; - } - - bool turn_signal_on_swerving = planner_data_->parameters.turn_signal_on_swerving; - - TurnSignalInfo turn_signal_info{}; - if (turn_signal_on_swerving) { - 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; - } - } else { - 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 { - turn_signal_info.desired_start_point = blinker_start_pose; - } - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - - return turn_signal_info; -} - void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; @@ -1489,7 +1390,8 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); + const auto min_return_distance = + helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point @@ -1700,7 +1602,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); - if (current_target_velocity < getEgoSpeed() && decel_distance < remaining_distance) { + if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::avoidance::insertDecelPoint( getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, slow_pose_); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 4332008b95e70..dabb0d7257555 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -19,7 +19,7 @@ #include -#include +#include namespace behavior_path_planner::utils::avoidance { @@ -890,7 +890,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { continue; } @@ -1101,12 +1101,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); + double prepare_distance_scaled = + std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1219,7 +1221,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { break; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 55fa1dd20dd79..f72f8279d4351 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -21,29 +21,23 @@ #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.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 @@ -53,6 +47,7 @@ namespace behavior_path_planner::utils::avoidance using autoware_perception_msgs::msg::TrafficSignalElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; +using geometry_msgs::msg::TransformStamped; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -65,7 +60,6 @@ using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; using tier4_planning_msgs::msg::AvoidanceDebugFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsg; @@ -231,6 +225,98 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } +bool existShiftSideLane( + const double start_shift_length, const double end_shift_length, const bool no_left_lanes, + const bool no_right_lanes) +{ + constexpr double THRESHOLD = 0.1; + const auto relative_shift_length = end_shift_length - start_shift_length; + + const auto avoid_shift = + std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD; + if (avoid_shift) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + } + + const auto return_shift = + std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD; + if (return_shift) { + // Right return. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD; + if (left_middle_shift) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD; + if (right_middle_shift) { + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + } + + return true; +} + +bool straddleRoadBound( + const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { + const auto transform = pose2transform(path.path.points.at(i).point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; +} + } // namespace namespace filtering_utils @@ -333,6 +419,34 @@ bool isParallelToEgoLane(const ObjectData & object, const double threshold) return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } +bool isMergingToEgoLane(const ObjectData & object) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + + if (isOnRight(object)) { + if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { + return false; + } + + if (yaw_deviation > M_PI_2) { + return false; + } + } else { + if (yaw_deviation > 0.0 && M_PI_2 > yaw_deviation) { + return false; + } + + if (yaw_deviation < -1.0 * M_PI_2) { + return false; + } + } + + return true; +} + bool isObjectOnRoadShoulder( ObjectData & object, const std::shared_ptr & route_handler, const std::shared_ptr & parameters) @@ -463,6 +577,15 @@ bool isForceAvoidanceTarget( return false; } + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->force_avoidance_distance_threshold; + + if (is_moving_distance_longer_than_threshold) { + return false; + } + if (object.is_within_intersection) { RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); return false; @@ -478,7 +601,6 @@ bool isForceAvoidanceTarget( } const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle bool not_parked_object = true; @@ -579,6 +701,17 @@ bool isSatisfiedWithNonVehicleCondition( return true; } +ObjectData::Behavior getObjectBehavior( + ObjectData & object, const std::shared_ptr & parameters) +{ + if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + return ObjectData::Behavior::NONE; + } + + return isMergingToEgoLane(object) ? ObjectData::Behavior::MERGING + : ObjectData::Behavior::DEVIATING; +} + bool isSatisfiedWithVehicleCondition( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, @@ -586,6 +719,7 @@ bool isSatisfiedWithVehicleCondition( { using boost::geometry::within; + object.behavior = getObjectBehavior(object, parameters); object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); // from here condition check for vehicle type objects. @@ -593,12 +727,7 @@ bool isSatisfiedWithVehicleCondition( return true; } - // Object is on center line -> ignore. - if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; - return false; - } - + // check vehicle shift ratio lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); const auto on_ego_driving_lane = within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); @@ -611,12 +740,26 @@ bool isSatisfiedWithVehicleCondition( } } - if (!object.is_within_intersection) { - return true; + // Object is on center line -> ignore. + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; } - if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { - object.reason = "ParallelToEgoLane"; + if (object.is_within_intersection) { + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "straight") { + return true; + } + + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + return false; + } + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; return false; } @@ -678,7 +821,11 @@ std::optional getAvoidMargin( bool isOnRight(const ObjectData & obj) { - return obj.lateral < 0.0; + if (obj.direction == Direction::NONE) { + throw std::logic_error("object direction is not initialized. something wrong."); + } + + return obj.direction == Direction::RIGHT; } double calcShiftLength( @@ -960,9 +1107,8 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - const bool is_left = 0 < object.lateral; obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, is_left}); + {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; } @@ -1232,6 +1378,22 @@ void fillAvoidanceNecessity( object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects) +{ + const auto id = object_data.object.object_id; + const auto same_id_obj = std::find_if( + detected_objects.begin(), detected_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + if (same_id_obj != detected_objects.end()) { + object_data.init_pose = same_id_obj->init_pose; + return; + } + + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + detected_objects.push_back(object_data); +} + void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) @@ -1358,76 +1520,60 @@ void compensateDetectionLost( double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + [[maybe_unused]] const std::shared_ptr & parameters) { using lanelet::utils::to2D; + using tier4_autoware_utils::Point2d; const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - findNearestIndex(data.reference_path_rough.points, object_pose.position); - const auto object_closest_pose = - data.reference_path_rough.points.at(object_closest_index).point.pose; + findNearestIndex(data.reference_path.points, object_pose.position); + const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { return 0.0; } - double road_shoulder_distance = std::numeric_limits::max(); - - const bool get_left = isOnRight(object) && parameters->use_adjacent_lane; - const bool get_right = !isOnRight(object) && parameters->use_adjacent_lane; - const bool get_opposite = parameters->use_opposite_lane; - - lanelet::BasicPoint3d p_overhang( - object.overhang_pose.position.x, object.overhang_pose.position.y, - object.overhang_pose.position.z); - - lanelet::ConstLineString3d target_line{}; - - const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { - const auto lines = - rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); - const auto & line = isOnRight(object) ? lines.back() : lines.front(); - const auto d = boost::geometry::distance(object.envelope_poly, to2D(line.basicLineString())); - if (d < road_shoulder_distance) { - road_shoulder_distance = d; - target_line = line; + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto & p1_object = object.overhang_pose.position; + const auto p_tmp = + geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); + const auto p2_object = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + std::vector intersects; + for (size_t i = 1; i < bound.size(); i++) { + const auto p1_bound = + geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); + const auto p2_bound = + geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); + + const auto opt_intersect = + tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); + + if (!opt_intersect) { + continue; } - }; - // current lanelet - { - update_road_to_shoulder_distance(object.overhang_lanelet); - - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, object.overhang_lanelet, - object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); + intersects.push_back(opt_intersect.value()); } - // previous lanelet - lanelet::ConstLanelets previous_lanelet{}; - if (rh->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &previous_lanelet)) { - update_road_to_shoulder_distance(previous_lanelet.front()); - - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, previous_lanelet.front(), - object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); + if (intersects.empty()) { + return 0.0; } - // next lanelet - lanelet::ConstLanelet next_lanelet{}; - if (rh->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { - update_road_to_shoulder_distance(next_lanelet); + std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { + return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); + }); - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, next_lanelet, object.overhang_pose.position, - p_overhang, parameters->use_hatched_road_markings, parameters->use_intersection_areas); - } + object.nearest_bound_point = intersects.front(); - return road_shoulder_distance; + return calcDistance2d(p1_object, object.nearest_bound_point.value()); } void filterTargetObjects( @@ -1475,93 +1621,6 @@ void filterTargetObjects( } } -double extendToRoadShoulderDistanceWithPolygon( - const std::shared_ptr & rh, - const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, - const bool use_intersection_areas) -{ - // get expandable polygons for avoidance (e.g. hatched road markings) - std::vector expandable_polygons; - - const auto exist_polygon = [&](const auto & candidate_polygon) { - return std::any_of( - expandable_polygons.begin(), expandable_polygons.end(), - [&](const auto & polygon) { return polygon.id() == candidate_polygon.id(); }); - }; - - if (use_hatched_road_markings) { - for (const auto & point : target_line) { - const auto new_polygon_candidate = - utils::getPolygonByPoint(rh, point, "hatched_road_markings"); - - if (!!new_polygon_candidate && !exist_polygon(*new_polygon_candidate)) { - expandable_polygons.push_back(*new_polygon_candidate); - } - } - } - - if (use_intersection_areas) { - const std::string area_id_str = overhang_lanelet.attributeOr("intersection_area", "else"); - - if (area_id_str != "else") { - expandable_polygons.push_back( - rh->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id_str.c_str()))); - } - } - - if (expandable_polygons.empty()) { - return to_road_shoulder_distance; - } - - // calculate point laterally offset from overhang position to calculate intersection with - // polygon - Point lat_offset_overhang_pos; - { - auto arc_coordinates = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(target_line), lanelet::utils::to2D(overhang_basic_pose)); - arc_coordinates.distance = 0.0; - const auto closest_target_line_point = - lanelet::geometry::fromArcCoordinates(target_line, arc_coordinates); - - const double ratio = 100.0 / to_road_shoulder_distance; - lat_offset_overhang_pos.x = - closest_target_line_point.x() + (closest_target_line_point.x() - overhang_pos.x) * ratio; - lat_offset_overhang_pos.y = - closest_target_line_point.y() + (closest_target_line_point.y() - overhang_pos.y) * ratio; - } - - // update to_road_shoulder_distance with valid expandable polygon - double updated_to_road_shoulder_distance = to_road_shoulder_distance; - for (const auto & polygon : expandable_polygons) { - std::vector intersect_dist_vec; - for (size_t i = 0; i < polygon.size(); ++i) { - const auto polygon_current_point = - geometry_msgs::build().x(polygon[i].x()).y(polygon[i].y()).z(0.0); - const auto polygon_next_point = geometry_msgs::build() - .x(polygon[(i + 1) % polygon.size()].x()) - .y(polygon[(i + 1) % polygon.size()].y()) - .z(0.0); - - const auto intersect_pos = tier4_autoware_utils::intersect( - overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point); - if (intersect_pos) { - intersect_dist_vec.push_back(calcDistance2d(*intersect_pos, overhang_pos)); - } - } - - if (intersect_dist_vec.empty()) { - continue; - } - - std::sort(intersect_dist_vec.begin(), intersect_dist_vec.end()); - updated_to_road_shoulder_distance = - std::max(updated_to_road_shoulder_distance, intersect_dist_vec.back()); - } - return updated_to_road_shoulder_distance; -} - AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line) { AvoidLineArray ret{line}; @@ -1685,12 +1744,12 @@ lanelet::ConstLanelets getAdjacentLane( lanelet::ConstLanelets lanes{}; for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); + const auto opt_left_lane = rh->getLeftLanelet(lane, true, false); if (!is_right_shift && opt_left_lane) { lanes.push_back(opt_left_lane.value()); } - const auto opt_right_lane = rh->getRightLanelet(lane); + const auto opt_right_lane = rh->getRightLanelet(lane, true, false); if (is_right_shift && opt_right_lane) { lanes.push_back(opt_right_lane.value()); } @@ -1706,7 +1765,8 @@ lanelet::ConstLanelets getAdjacentLane( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift) + const std::shared_ptr & parameters, const bool is_right_shift, + DebugData & debug) { const auto & p = parameters; const auto check_right_lanes = @@ -1764,6 +1824,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check left lanes @@ -1783,6 +1846,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check current lanes @@ -1802,6 +1868,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } return target_objects; @@ -1883,7 +1952,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters, const bool in_avoidance_maneuver) { const auto & route_handler = planner_data->route_handler; @@ -1897,6 +1966,11 @@ DrivableLanes generateExpandDrivableLanes( // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_left_lanelets.empty()) { @@ -1907,6 +1981,11 @@ DrivableLanes generateExpandDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_right_lanelets.empty()) { @@ -2078,4 +2157,100 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } + +TurnSignalInfo calcTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, + const AvoidancePlanningData & data, const std::shared_ptr & planner_data) +{ + constexpr double THRESHOLD = 0.1; + const auto & p = planner_data->parameters; + const auto & rh = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x; + + if (shift_line.start_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.start_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.end_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.end_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + const auto start_shift_length = path.shift_length.at(shift_line.start_idx); + const auto end_shift_length = path.shift_length.at(shift_line.end_idx); + const auto relative_shift_length = end_shift_length - start_shift_length; + + // If shift length is shorter than the threshold, it does not need to turn on blinkers + if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { + return {}; + } + + // If the vehicle does not shift anymore, we turn off the blinker + if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { + return {}; + } + + const auto get_command = [](const auto & shift_length) { + return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT + : TurnIndicatorsCommand::ENABLE_RIGHT; + }; + + const auto signal_prepare_distance = + std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); + const auto ego_front_to_shift_start = + calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - + p.vehicle_info.max_longitudinal_offset_m; + + if (signal_prepare_distance < ego_front_to_shift_start) { + return {}; + } + + const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; + const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; + const auto get_start_pose = [&](const auto & ego_to_shift_start) { + return ego_to_shift_start ? ego_pose : blinker_start_pose; + }; + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); + turn_signal_info.desired_end_point = blinker_end_pose; + turn_signal_info.required_start_point = blinker_start_pose; + turn_signal_info.required_end_point = blinker_end_pose; + turn_signal_info.turn_signal.command = get_command(relative_shift_length); + + if (!p.turn_signal_on_swerving) { + return turn_signal_info; + } + + lanelet::ConstLanelet lanelet; + if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { + return {}; + } + + const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); + const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + + if (!existShiftSideLane( + start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + return {}; + } + + if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { + return {}; + } + + return turn_signal_info; +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 58bf36d978cea..8cf85554e2b2c 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" -#include +#include +#include +#include -#include #include using behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_avoidance_module/test/test_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp index e5c6fd2e72092..95488daa69cf8 100644 --- a/planning/behavior_path_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -25,7 +25,7 @@ using behavior_path_planner::utils::avoidance::isSameDirectionShift; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { ObjectData right_obj; - right_obj.lateral = -0.3; + right_obj.direction = route_handler::Direction::RIGHT; const double negative_shift_length = -1.0; const double positive_shift_length = 1.0; @@ -33,11 +33,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), positive_shift_length)); ObjectData left_obj; - left_obj.lateral = 0.3; + left_obj.direction = route_handler::Direction::LEFT; ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), positive_shift_length)); ASSERT_FALSE(isSameDirectionShift(isOnRight(left_obj), negative_shift_length)); - - const double zero_shift_length = 0.0; - ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), zero_shift_length)); - ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), zero_shift_length)); } diff --git a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..98874b8923324 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_dynamic_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_dynamic_avoidance_module/README.md similarity index 94% rename from planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md rename to planning/behavior_path_dynamic_avoidance_module/README.md index f303937bd0f78..5e8be9136886c 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md +++ b/planning/behavior_path_dynamic_avoidance_module/README.md @@ -32,16 +32,16 @@ First, a maximum lateral offset to avoid is calculated as follows. The polygon's width to extract from the drivable area is the obstacle width and double `drivable_area_generation.lat_offset_from_obstacle`. We can limit the lateral shift offset by `drivable_area_generation.max_lat_offset_to_avoid`. -![drivable_area_extraction_width](../image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg) +![drivable_area_extraction_width](./image/drivable_area_extraction_width.drawio.svg) Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision). Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g. The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.). Same directional obstacles -![same_directional_object](../image/dynamic_avoidance/same_directional_object.svg) +![same_directional_object](./image/same_directional_object.svg) Opposite directional obstacles -![opposite_directional_object](../image/dynamic_avoidance/opposite_directional_object.svg) +![opposite_directional_object](./image/opposite_directional_object.svg) ## Parameters diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml similarity index 89% rename from planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml rename to planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml index 854c29aa89bc5..371f7da2fadf5 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml @@ -46,6 +46,9 @@ min_object_vel: -0.5 # [m/s] The value is negative considering the noisy velocity of the stopped vehicle. max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value. + stopped_object: + max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value. + drivable_area_generation: polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base" object_path_base: @@ -56,6 +59,10 @@ max_time_for_object_lat_shift: 0.0 # [s] lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] + max_ego_lat_acc: 0.3 # [m/ss] + max_ego_lat_jerk: 0.3 # [m/sss] + delay_time_ego_shift: 1.0 # [s] + # for same directional object overtaking_object: max_time_to_collision: 40.0 # [s] diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg rename to planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp index c02ee88d3fa3e..c2c17c4e55c1e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -44,10 +44,12 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + private: std::shared_ptr parameters_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp similarity index 71% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index e37ef27d44426..d2f9cd95066d9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include #include @@ -35,6 +32,25 @@ #include #include +namespace +{ +template +bool isInVector(const T & val, const std::vector & vec) +{ + return std::find(vec.begin(), vec.end(), val) != vec.end(); +} + +template +std::vector getAllKeys(const std::unordered_map & map) +{ + std::vector keys; + for (const auto & pair : map) { + keys.push_back(pair.first); + } + return keys; +} +} // namespace + namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedPath; @@ -87,6 +103,7 @@ struct DynamicAvoidanceParameters double max_overtaking_crossing_object_angle{0.0}; double min_oncoming_crossing_object_vel{0.0}; double max_oncoming_crossing_object_angle{0.0}; + double max_stopped_object_vel{0.0}; // drivable area generation PolygonGenerationMethod polygon_generation_method{}; @@ -96,6 +113,10 @@ struct DynamicAvoidanceParameters double max_time_for_lat_shift{0.0}; double lpf_gain_for_lat_avoid_to_offset{0.0}; + double max_ego_lat_acc{0.0}; + double max_ego_lat_jerk{0.0}; + double delay_time_ego_shift{0.0}; + double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; double end_duration_to_avoid_overtaking_object{0.0}; @@ -106,6 +127,17 @@ struct DynamicAvoidanceParameters double end_duration_to_avoid_oncoming_object{0.0}; }; +struct TimeWhileCollision +{ + double time_to_start_collision; + double time_to_end_collision; +}; + +struct LatFeasiblePaths +{ + std::vector left_path; + std::vector right_path; +}; class DynamicAvoidanceModule : public SceneModuleInterface { public: @@ -143,15 +175,19 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left{false}; bool should_be_avoided{false}; + std::vector ref_path_points_for_obj_poly; + LatFeasiblePaths ego_lat_feasible_paths; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided) + const bool arg_is_collision_left, const bool arg_should_be_avoided, + const std::vector & arg_ref_path_points_for_obj_poly) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; + ref_path_points_for_obj_poly = arg_ref_path_points_for_obj_poly; } }; @@ -162,14 +198,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface { } int max_count_{0}; - int min_count_{0}; + int min_count_{0}; // TODO(murooka): The sign needs to be opposite? void initialize() { current_uuids_.clear(); } void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { // add/update object if (object_map_.count(uuid) != 0) { + const auto prev_object = object_map_.at(uuid); object_map_.at(uuid) = object; + // TODO(murooka) refactor this. Counter can be moved to DynamicObject, + // and TargetObjectsManager can be removed. + object_map_.at(uuid).ego_lat_feasible_paths = prev_object.ego_lat_feasible_paths; } else { object_map_.emplace(uuid, object); } @@ -185,14 +225,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface current_uuids_.push_back(uuid); } - void finalize() + void finalize(const LatFeasiblePaths & ego_lat_feasible_paths) { // decrease counter for not updated uuids std::vector not_updated_uuids; for (const auto & object : object_map_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), object.first) == - current_uuids_.end()) { + if (!isInVector(object.first, current_uuids_)) { not_updated_uuids.push_back(object.first); } } @@ -204,52 +242,55 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - // remove objects whose counter is lower than threshold - std::vector obsolete_uuids; + // update valid object uuids and its variable for (const auto & counter : counter_map_) { - if (counter.second < min_count_) { - obsolete_uuids.push_back(counter.first); + if (!isInVector(counter.first, valid_object_uuids_) && max_count_ <= counter.second) { + valid_object_uuids_.push_back(counter.first); + object_map_.at(counter.first).ego_lat_feasible_paths = ego_lat_feasible_paths; } } - for (const auto & obsolete_uuid : obsolete_uuids) { - counter_map_.erase(obsolete_uuid); - object_map_.erase(obsolete_uuid); + valid_object_uuids_.erase( + std::remove_if( + valid_object_uuids_.begin(), valid_object_uuids_.end(), + [&](const auto & uuid) { + return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_; + }), + valid_object_uuids_.end()); + + // remove objects whose counter is lower than threshold + const auto counter_map_keys = getAllKeys(counter_map_); + for (const auto & key : counter_map_keys) { + if (counter_map_.at(key) < min_count_) { + counter_map_.erase(key); + object_map_.erase(key); + } } } std::vector getValidObjects() const { - std::vector objects; - for (const auto & object : object_map_) { - if (counter_map_.count(object.first) != 0) { - if (max_count_ <= counter_map_.at(object.first)) { - objects.push_back(object.second); - } + std::vector valid_objects; + for (const auto & valid_object_uuid : valid_object_uuids_) { + if (object_map_.count(valid_object_uuid) == 0) { + std::cerr + << "[DynamicAvoidance] Internal calculation has an error when getting valid objects." + << std::endl; + continue; } + valid_objects.push_back(object_map_.at(valid_object_uuid)); } - return objects; - } - std::optional getValidObject(const std::string & uuid) const - { - // add/update object - if (counter_map_.count(uuid) == 0) { - return std::nullopt; - } - if (counter_map_.at(uuid) < max_count_) { - return std::nullopt; - } - if (object_map_.count(uuid) == 0) { - return std::nullopt; - } - return object_map_.at(uuid); + + return valid_objects; } - void updateObject( + void updateObjectVariables( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided) + const bool should_be_avoided, + const std::vector & ref_path_points_for_obj_poly) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + ref_path_points_for_obj_poly); } } @@ -257,6 +298,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface // NOTE: positive is for meeting entry condition, and negative is for exiting. std::unordered_map counter_map_; std::unordered_map object_map_; + std::vector valid_object_uuids_{}; }; struct DecisionWithReason @@ -307,34 +349,41 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); + LatFeasiblePaths generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const; + void updateRefPathBeforeLaneChange(const std::vector & ego_ref_path_points); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; + bool willObjectBeOutsideEgoChangingPath( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; - double calcTimeToCollision( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + TimeWhileCollision calcTimeWhileCollision( + const std::vector & ego_path, const double obj_tangent_vel, + const LatLonOffset & lat_lon_offset) const; std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; - double calcValidStartLengthToAvoid( + double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const bool is_object_same_direction) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & input_ref_path_points, + const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const double time_to_collision) const; + const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( - const std::vector & input_ref_path_points, const Polygon2d & obj_points, - const double obj_vel, const bool is_collision_left, const double obj_normal_vel, - const std::optional & prev_object) const; + const std::vector & ref_path_points_for_obj_poly, + const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, + const double obj_normal_vel, const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; @@ -353,11 +402,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::vector target_objects_; // std::vector prev_target_objects_; - std::vector prev_input_ref_path_points; + std::optional> prev_input_ref_path_points_{std::nullopt}; + std::optional> ref_path_before_lane_change_{std::nullopt}; std::shared_ptr parameters_; TargetObjectsManager target_objects_manager_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/package.xml b/planning/behavior_path_dynamic_avoidance_module/package.xml new file mode 100644 index 0000000000000..11792a15b2cd2 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_path_dynamic_avoidance_module + 0.1.0 + The behavior_path_dynamic_avoidance_module package + + Takayuki Murooka + Yuki Takagi + Satoshi Ota + Kosuke Takeuchi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils + object_recognition_utils + pluginlib + rclcpp + signal_processing + tier4_autoware_utils + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_dynamic_avoidance_module/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..fd2e1bc4137b7 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp similarity index 93% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/manager.cpp index 2824a6221591a..569c6a17b5b32 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_dynamic_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -99,6 +99,9 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel"); p.max_oncoming_crossing_object_angle = node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle"); + + p.max_stopped_object_vel = + node->declare_parameter(ns + "stopped_object.max_object_vel"); } { // drivable_area_generation @@ -114,6 +117,10 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) p.lpf_gain_for_lat_avoid_to_offset = node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); + p.max_ego_lat_acc = node->declare_parameter(ns + "max_ego_lat_acc"); + p.max_ego_lat_jerk = node->declare_parameter(ns + "max_ego_lat_jerk"); + p.delay_time_ego_shift = node->declare_parameter(ns + "delay_time_ego_shift"); + p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); p.start_duration_to_avoid_overtaking_object = @@ -207,6 +214,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "crossing_object.max_oncoming_object_angle", p->max_oncoming_crossing_object_angle); + + updateParam( + parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel); } { // drivable_area_generation @@ -227,6 +237,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); + updateParam(parameters, ns + "max_ego_lat_acc", p->max_ego_lat_acc); + updateParam(parameters, ns + "max_ego_lat_jerk", p->max_ego_lat_jerk); + updateParam(parameters, ns + "delay_time_ego_shift", p->delay_time_ego_shift); + updateParam( parameters, ns + "overtaking_object.max_time_to_collision", p->max_time_to_collision_overtaking_object); @@ -255,6 +269,11 @@ void DynamicAvoidanceModuleManager::updateModuleParams( if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } + +bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const +{ + return true; +} } // namespace behavior_path_planner #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp similarity index 63% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index de76166e068d3..ca58bbf0adcaa 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -137,7 +137,26 @@ double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & s return max_length_to_point; } - throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); + throw std::logic_error("The shape type is not supported in dynamic_avoidance."); +} + +double calcObstacleWidth(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return shape.dimensions.y; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x; + } 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 dynamic_avoidance."); } double calcDiffAngleAgainstPath( @@ -153,7 +172,7 @@ double calcDiffAngleAgainstPath( return diff_yaw; } -double calcDiffAngleBetweenPaths( +[[maybe_unused]] double calcDiffAngleBetweenPaths( const std::vector & path_points, const PredictedPath & predicted_path) { const size_t nearest_idx = @@ -243,6 +262,35 @@ double calcDistanceToSegment( first_to_second * first_to_target.dot(first_to_second) / std::pow(first_to_second.norm(), 2); return (Eigen::Vector2d{p1.x, p1.y} - p2_nearest).norm(); } + +std::optional> intersectLines( + const std::vector & source_line, + const std::vector & target_line) +{ + for (int source_seg_idx = 0; source_seg_idx < static_cast(source_line.size()) - 1; + ++source_seg_idx) { + for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; + ++target_seg_idx) { + const auto intersect_point = tier4_autoware_utils::intersect( + source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, + target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); + if (intersect_point) { + return std::make_pair(source_seg_idx, *intersect_point); + } + } + } + return std::nullopt; +} + +std::vector convertToPoints( + const std::vector & poses) +{ + std::vector points; + for (const auto & pose : poses) { + points.push_back(pose.position); + } + return points; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( @@ -264,12 +312,12 @@ bool DynamicAvoidanceModule::isExecutionRequested() const RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); const auto input_path = getPreviousModuleOutput().path; - if (!input_path || input_path->points.size() < 2) { + if (input_path.points.size() < 2) { return false; } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(input_path->points); + const auto is_driving_forward = motion_utils::isDrivingForward(input_path.points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -291,6 +339,9 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { + info_marker_.markers.clear(); + debug_marker_.markers.clear(); + // calculate target objects updateTargetObjects(); @@ -310,9 +361,6 @@ bool DynamicAvoidanceModule::canTransitSuccessState() BehaviorModuleOutput DynamicAvoidanceModule::plan() { - info_marker_.markers.clear(); - debug_marker_.markers.clear(); - const auto & input_path = getPreviousModuleOutput().path; // create obstacles to avoid (= extract from the drivable area) @@ -401,9 +449,15 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto input_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + const auto input_ref_path_points = getPreviousModuleOutput().reference_path.points; const auto prev_objects = target_objects_manager_.getValidObjects(); + updateRefPathBeforeLaneChange(input_ref_path_points); + + const auto & ego_pose = getEgoPose(); + const double ego_vel = getEgoSpeed(); + const auto ego_lat_feasible_paths = generateLateralFeasiblePaths(ego_pose, ego_vel); + // 1. Rough filtering of target objects target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { @@ -427,7 +481,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(input_path->points, predicted_object); + projectObstacleVelocityToTrajectory(input_path.points, predicted_object); if ( std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { @@ -435,7 +489,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleBetweenPaths(input_path->points, obj_path); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, obj_pose); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -455,7 +509,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.e. check if object lateral offset to ego's path is small enough - const double obj_dist_to_path = calcDistanceToPath(input_path->points, obj_pose.position); + const double obj_dist_to_path = calcDistanceToPath(input_path.points, obj_pose.position); const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { RCLCPP_INFO_EXPRESSION( @@ -488,7 +542,7 @@ void DynamicAvoidanceModule::updateTargetObjects() latest_time_inside_ego_path); target_objects_manager_.updateObject(obj_uuid, target_object); } - target_objects_manager_.finalize(); + target_objects_manager_.finalize(ego_lat_feasible_paths); // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { @@ -498,8 +552,10 @@ void DynamicAvoidanceModule::updateTargetObjects() object.predicted_paths.begin(), object.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const auto & ref_path_points_for_obj_poly = input_path.points; + // 2.a. check if object is not to be followed by ego - const double obj_angle = calcDiffAngleAgainstPath(input_path->points, object.pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, object.pose); const bool is_object_aligned_to_path = std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); @@ -513,13 +569,13 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.b. calculate which side object exists against ego's path - const bool is_object_left = isLeft(input_path->points, object.pose.position); + const bool is_object_left = isLeft(input_path.points, object.pose.position); const auto lat_lon_offset = - getLateralLongitudinalOffset(input_path->points, object.pose, object.shape); + getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); // 2.c. check if object will not cut in const bool will_object_cut_in = - willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset); + willObjectCutIn(input_path.points, obj_path, object.vel, lat_lon_offset); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -535,39 +591,66 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } + // 2.e. check if the ego will change the lane and the object will be outside the ego's path + // const auto will_object_be_outside_ego_changing_path = + // willObjectBeOutsideEgoChangingPath(object.pose, object.shape, object.vel); + // if (will_object_be_outside_ego_changing_path) { + // RCLCPP_INFO_EXPRESSION( + // getLogger(), parameters_->enable_debug_info, + // "[DynamicAvoidance] Ignore obstacle (%s) since the object will be outside ego's changing + // path", obj_uuid.c_str()); + // continue; + // } + // 2.e. check time to collision - const double time_to_collision = - calcTimeToCollision(input_path->points, object.pose, object.vel, lat_lon_offset); - if ( - (0 <= object.vel && - parameters_->max_time_to_collision_overtaking_object < time_to_collision) || - (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", - obj_uuid.c_str(), time_to_collision); - continue; - } - if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative " - "value.", - obj_uuid.c_str(), time_to_collision); - continue; + const auto time_while_collision = + calcTimeWhileCollision(input_path.points, object.vel, lat_lon_offset); + const double time_to_collision = time_while_collision.time_to_start_collision; + if (parameters_->max_stopped_object_vel < std::hypot(object.vel, object.lat_vel)) { + // NOTE: Only not stopped object is filtered by time to collision. + if ( + (0 <= object.vel && + parameters_->max_time_to_collision_overtaking_object < time_to_collision) || + (object.vel <= 0 && + parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is large.", + obj_uuid.c_str(), time_to_collision_str.c_str()); + continue; + } + if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is a small " + "negative value.", + obj_uuid.c_str(), time_to_collision_str.c_str()); + continue; + } } // 2.f. calculate which side object will be against ego's path - const auto future_obj_pose = - object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); - const bool is_collision_left = - future_obj_pose ? isLeft(input_path->points, future_obj_pose->position) : is_object_left; + const bool is_collision_left = [&]() { + if (0.0 < object.vel) { + return is_object_left; + } + const auto future_obj_pose = + object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); + return future_obj_pose ? isLeft(input_path.points, future_obj_pose->position) + : is_object_left; + }(); // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path->points); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - input_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path.points, ego_pose.position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -590,10 +673,10 @@ void DynamicAvoidanceModule::updateTargetObjects() // "ego_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - input_ref_path_points, object.pose, obj_points, object.vel, obj_path, object.shape, - time_to_collision); + ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, + time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - input_ref_path_points, obj_points, object.vel, is_collision_left, object.lat_vel, + ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, prev_object); if (!lat_offset_to_avoid) { RCLCPP_INFO_EXPRESSION( @@ -605,11 +688,74 @@ void DynamicAvoidanceModule::updateTargetObjects() } const bool should_be_avoided = true; - target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided); + target_objects_manager_.updateObjectVariables( + obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, + ref_path_points_for_obj_poly); + } + + prev_input_ref_path_points_ = input_ref_path_points; +} + +LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const +{ + const double lat_acc = parameters_->max_ego_lat_acc; + const double lat_jerk = parameters_->max_ego_lat_jerk; + const double delay_time = parameters_->delay_time_ego_shift; + + LatFeasiblePaths ego_lat_feasible_paths; + for (double t = 0; t < 10.0; t += 1.0) { + const double feasible_lat_offset = lat_acc * std::pow(std::max(t - delay_time, 0.0), 2) / 2.0 + + lat_jerk * std::pow(std::max(t - delay_time, 0.0), 3) / 6.0 - + planner_data_->parameters.vehicle_width / 2.0; + const double x = t * ego_vel; + const double y = feasible_lat_offset; + + const auto feasible_left_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); + + const auto feasible_right_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); } - prev_input_ref_path_points = input_ref_path_points; + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + + return ego_lat_feasible_paths; +} + +void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( + const std::vector & ego_ref_path_points) +{ + if (ref_path_before_lane_change_) { + // check if the ego is close enough to the current ref path, meaning that lane change ends. + const auto ego_pos = getEgoPose().position; + const double dist_to_ref_path = + std::abs(motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); + + constexpr double epsilon_dist_to_ref_path = 0.5; + if (dist_to_ref_path < epsilon_dist_to_ref_path) { + ref_path_before_lane_change_ = std::nullopt; + } + } else { + // check if the ego is during lane change. + if (prev_input_ref_path_points_ && !prev_input_ref_path_points_->empty()) { + const double dist_ref_paths = std::abs(motion_utils::calcLateralOffset( + ego_ref_path_points, prev_input_ref_path_points_->front().point.pose.position)); + constexpr double epsilon_ref_paths_diff = 1.0; + if (epsilon_ref_paths_diff < dist_ref_paths) { + ref_path_before_lane_change_ = *prev_input_ref_path_points_; + } + } + } } [[maybe_unused]] std::optional> @@ -650,26 +796,60 @@ DynamicAvoidanceModule::calcCollisionSection( return std::make_pair(*collision_start_idx, ego_path.size() - 1); } -double DynamicAvoidanceModule::calcTimeToCollision( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const +TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( + const std::vector & ego_path, const double obj_tangent_vel, + const LatLonOffset & lat_lon_offset) const { // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); - const double lon_offset_ego_to_obj = - motion_utils::calcSignedArcLength( - ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + - lat_lon_offset.max_lon_offset; - const double maximum_time_to_collision = - (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits::max(); - + const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; - const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); - const double signed_lon_length = motion_utils::calcSignedArcLength( - ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); - const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); + + const double signed_time_to_start_collision = [&]() { + const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx + + lat_lon_offset.min_lon_offset - + planner_data_->parameters.front_overhang; + const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx - + lat_lon_offset.max_lon_offset - + planner_data_->parameters.rear_overhang; + if (0.0 < lon_offset_ego_front_to_obj_back) { // The object is ahead of the ego. + return lon_offset_ego_front_to_obj_back / relative_velocity; + } else if (0.0 < lon_offset_obj_front_to_ego_back) { // The ego is ahead of the object. + return lon_offset_obj_front_to_ego_back / -relative_velocity; + } + // The ego and object are colliding. + return 0.0; + }(); + const double signed_time_to_end_collision = [&]() { + const double lon_offset_ego_back_to_obj_front = lon_offset_ego_to_obj_idx + + lat_lon_offset.max_lon_offset + + planner_data_->parameters.rear_overhang; + const double lon_offset_obj_back_to_ego_front = -lon_offset_ego_to_obj_idx - + lat_lon_offset.min_lon_offset + + planner_data_->parameters.front_overhang; + if (0.0 < relative_velocity) { + return lon_offset_ego_back_to_obj_front / relative_velocity; + } + return lon_offset_obj_back_to_ego_front / -relative_velocity; + }(); + + // NOTE: In order to make time_to_start_collision continuous around the relative_velocity is zero. + const double time_to_start_collision = [&]() { + if (signed_time_to_start_collision < 0.0) { + return std::numeric_limits::max(); + } + return signed_time_to_start_collision; + }(); + const double time_to_end_collision = [&]() { + if (signed_time_to_end_collision < 0.0) { + return std::numeric_limits::max(); + } + return signed_time_to_end_collision; + }(); + + return TimeWhileCollision{time_to_start_collision, time_to_end_collision}; } bool DynamicAvoidanceModule::isObjectFarFromPath( @@ -765,6 +945,25 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return DecisionWithReason{false}; } +[[maybe_unused]] bool DynamicAvoidanceModule::willObjectBeOutsideEgoChangingPath( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const +{ + if (!ref_path_before_lane_change_ || obj_vel < 0.0) { + return false; + } + + // Check if object is in the lane before ego's lane change. + const double dist_to_ref_path_before_lane_change = + std::abs(motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); + const double epsilon_dist_checking_in_lane = calcObstacleWidth(obj_shape); + if (epsilon_dist_checking_in_lane < dist_to_ref_path_before_lane_change) { + return false; + } + + return true; +} + std::pair DynamicAvoidanceModule::getAdjacentLanes( const double forward_distance, const double backward_distance) const { @@ -840,13 +1039,13 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi } MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & input_ref_path_points, + const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const double time_to_collision) const + const TimeWhileCollision & time_while_collision) const { const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, obj_pose.position); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { @@ -854,67 +1053,104 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - input_ref_path_points, obj_seg_idx, geom_obj_point); + ref_path_points_for_obj_poly, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } - const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec); + return getMinMaxValues(obj_lon_offset_vec); + }(); + + const double relative_velocity = getEgoSpeed() - obj_vel; - if (obj_vel < 0) { - return MinMaxValue{ - raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; + // calculate bound start and end length + const double start_length_to_avoid = [&]() { + if (obj_vel < parameters_->max_stopped_object_vel) { + // The ego and object are the same directional or the object is parked. + return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; } - - // NOTE: If time to collision is considered here, the ego is close to the object when starting - // avoidance. - // The ego should start avoidance before overtaking. - return raw_obj_lon_offset; + // The ego and object are the opposite directional. + const double obj_acc = -1.0; + const double decel_time = 1.0; + const double obj_moving_dist = + (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / + obj_acc; + const double ego_moving_dist = getEgoSpeed() * decel_time; + return std::max(0.0, ego_moving_dist - obj_moving_dist) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; + }(); + const double end_length_to_avoid = [&]() { + if (obj_vel < parameters_->max_stopped_object_vel) { + // The ego and object are the same directional or the object is parked. + return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object; + } + // The ego and object are the opposite directional. + return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + + std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; }(); - // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= obj_vel); - // TODO(murooka) use getEgoSpeed() instead of obj_vel - const double start_length_to_avoid = - std::abs(obj_vel) * (is_object_overtaking - ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); - const double end_length_to_avoid = - std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); - - if (obj_vel < 0) { - const double valid_start_length_to_avoid = - calcValidStartLengthToAvoid(obj_path, obj_pose, obj_shape); + // calculate valid path for the forked object's path from the ego's path + if (obj_vel < -parameters_->max_stopped_object_vel) { + const bool is_object_same_direction = false; + const double valid_length_to_avoid = + calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction); return MinMaxValue{ - std::max(obj_lon_offset.min_value - start_length_to_avoid, valid_start_length_to_avoid), + obj_lon_offset.min_value + std::max(-start_length_to_avoid, -valid_length_to_avoid), obj_lon_offset.max_value + end_length_to_avoid}; } + if (parameters_->max_stopped_object_vel < obj_vel) { + const bool is_object_same_direction = true; + const double valid_length_to_avoid = + calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction); + return MinMaxValue{ + obj_lon_offset.min_value - start_length_to_avoid, + obj_lon_offset.max_value + std::min(end_length_to_avoid, valid_length_to_avoid)}; + } return MinMaxValue{ obj_lon_offset.min_value - start_length_to_avoid, obj_lon_offset.max_value + end_length_to_avoid}; } -double DynamicAvoidanceModule::calcValidStartLengthToAvoid( +double DynamicAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const + const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const bool is_object_same_direction) const { - const auto input_path_points = getPreviousModuleOutput().path->points; + const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); + const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 + + parameters_->lat_offset_from_obstacle + + calcObstacleMaxLength(obj_shape); + + // crop the ego's path by object position + std::vector cropped_ego_path_points; + if (is_object_same_direction) { + cropped_ego_path_points = std::vector{ + input_path_points.begin() + obj_seg_idx, input_path_points.end()}; + } else { + cropped_ego_path_points = std::vector{ + input_path_points.begin(), input_path_points.begin() + obj_seg_idx + 1 + 1}; + std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end()); + } + if (cropped_ego_path_points.size() < 2) { + return motion_utils::calcArcLength(obj_path.path); + } + + // calculate where the object's path will be forked from (= far from) the ego's path. + std::optional last_nearest_ego_path_seg_idx{std::nullopt}; const size_t valid_obj_path_end_idx = [&]() { - int ego_path_idx = obj_seg_idx + 1; + size_t ego_path_seg_idx = 0; for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) { bool are_paths_close{false}; - for (; 0 < ego_path_idx; --ego_path_idx) { + for (; ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) { const double dist_to_segment = calcDistanceToSegment( obj_path.path.at(obj_path_idx).position, - input_path_points.at(ego_path_idx).point.pose.position, - input_path_points.at(ego_path_idx - 1).point.pose.position); - if ( - dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 + - parameters_->lat_offset_from_obstacle + - calcObstacleMaxLength(obj_shape)) { + cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position, + cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position); + if (dist_to_segment < dist_threshold_paths) { + last_nearest_ego_path_seg_idx = ego_path_seg_idx; are_paths_close = true; break; } @@ -926,31 +1162,70 @@ double DynamicAvoidanceModule::calcValidStartLengthToAvoid( } return obj_path.path.size() - 1; }(); - return -motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); + + // calculate valid length to avoid + if (last_nearest_ego_path_seg_idx && valid_obj_path_end_idx != obj_path.path.size() - 1) { + const auto calc_min_dist = [&](const size_t arg_obj_path_idx) -> std::optional { + std::optional min_dist{std::nullopt}; + for (size_t ego_path_seg_idx = *last_nearest_ego_path_seg_idx; + ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) { + const double dist_to_segment = calcDistanceToSegment( + obj_path.path.at(arg_obj_path_idx).position, + cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position, + cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position); + if (!min_dist || dist_to_segment < *min_dist) { + min_dist = dist_to_segment; + } + if (min_dist && *min_dist < dist_to_segment) { + return *min_dist; + } + } + return min_dist; + }; + const size_t prev_valid_obj_path_end_idx = + (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx : valid_obj_path_end_idx - 1; + const size_t next_valid_obj_path_end_idx = + (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx + 1 : valid_obj_path_end_idx; + const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); + const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); + if (prev_min_dist && next_min_dist) { + const double segment_length = tier4_autoware_utils::calcDistance2d( + obj_path.path.at(prev_valid_obj_path_end_idx), + obj_path.path.at(next_valid_obj_path_end_idx)); + const double partial_segment_length = segment_length * + (dist_threshold_paths - *prev_min_dist) / + (*next_min_dist - *prev_min_dist); + return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) + + partial_segment_length; + } + } + return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); } std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( - const std::vector & input_ref_path_points, const Polygon2d & obj_points, - const double obj_vel, const bool is_collision_left, const double obj_normal_vel, - const std::optional & prev_object) const + const std::vector & ref_path_points_for_obj_poly, + const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, + const double obj_normal_vel, const std::optional & prev_object) const { + const bool enable_lowpass_filter = true; + /* const bool enable_lowpass_filter = [&]() { - if (prev_input_ref_path_points.size() < 2) { + if (!prev_ref_path_points_for_obj_poly_ || prev_ref_path_points_for_obj_poly_->size() < 2) { return true; } const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex( - prev_input_ref_path_points, input_ref_path_points.front().point.pose.position); - constexpr double min_lane_change_path_lat_offset = 1.0; - if ( - motion_utils::calcLateralOffset( - prev_input_ref_path_points, input_ref_path_points.front().point.pose.position, - prev_front_seg_idx) < min_lane_change_path_lat_offset) { - return true; + *prev_ref_path_points_for_obj_poly_, + ref_path_points_for_obj_poly.front().point.pose.position); constexpr double + min_lane_change_path_lat_offset = 1.0; if ( motion_utils::calcLateralOffset( + *prev_ref_path_points_for_obj_poly_, + ref_path_points_for_obj_poly.front().point.pose.position, prev_front_seg_idx) < + min_lane_change_path_lat_offset) { return true; } // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to // shift the obstacle polygon suddenly. return false; }(); + */ // calculate min/max lateral offset from object to path const auto obj_lat_abs_offset = [&]() { @@ -958,9 +1233,9 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, geom_obj_point); - const double obj_point_lat_offset = - motion_utils::calcLateralOffset(input_ref_path_points, geom_obj_point, obj_point_seg_idx); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point); + const double obj_point_lat_offset = motion_utils::calcLateralOffset( + ref_path_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } return getMinMaxValues(obj_lat_abs_offset_vec); @@ -1027,19 +1302,19 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( return std::nullopt; } - auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + auto ref_path_points_for_obj_poly = object.ref_path_points_for_obj_poly; const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, object.pose.position); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid->min_value, input_ref_path_points); + obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, input_ref_path_points); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_path_points_for_obj_poly); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -1049,33 +1324,76 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(input_ref_path_points.size() - 1); + : static_cast(ref_path_points_for_obj_poly.size() - 1); - // create inner/outer bound points - std::vector obj_inner_bound_points; - std::vector obj_outer_bound_points; + // create inner bound points + std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( - input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0) - .position); - obj_outer_bound_points.push_back( + // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. + obj_inner_bound_poses.push_back(tier4_autoware_utils::calcOffsetPose( + ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, + 0.0)); + } + + // calculate start index laterally feasible for ego to shift considering maximum lateral jerk and + // acceleration + const auto obj_inner_bound_start_idx = [&]() -> std::optional { + const auto & ego_lat_feasible_path = object.is_collision_left + ? object.ego_lat_feasible_paths.left_path + : object.ego_lat_feasible_paths.right_path; + const auto intersect_result = intersectLines(obj_inner_bound_poses, ego_lat_feasible_path); + + // Check if the object polygon intersects with the ego_lat_feasible_path. + if (intersect_result) { + const auto & [bound_seg_idx, intersect_point] = *intersect_result; + const double lon_offset = tier4_autoware_utils::calcDistance2d( + obj_inner_bound_poses.at(bound_seg_idx), intersect_point); + + const auto obj_inner_bound_start_idx_opt = + motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); + if (obj_inner_bound_start_idx_opt) { + return *obj_inner_bound_start_idx_opt; + } + } + + // Check if the object polygon is fully outside the ego_lat_feasible_path. + const double obj_poly_lat_offset = motion_utils::calcLateralOffset( + ego_lat_feasible_path, obj_inner_bound_poses.front().position); + if ( + (!object.is_collision_left && 0 < obj_poly_lat_offset) || + (object.is_collision_left && obj_poly_lat_offset < 0)) { + return std::nullopt; + } + + return 0; + }(); + if (!obj_inner_bound_start_idx) { + return std::nullopt; + } + + // calculate feasible inner/outer bound points + const auto feasible_obj_inner_bound_poses = std::vector( + obj_inner_bound_poses.begin() + *obj_inner_bound_start_idx, obj_inner_bound_poses.end()); + const auto feasible_obj_inner_bound_points = convertToPoints(feasible_obj_inner_bound_poses); + std::vector feasible_obj_outer_bound_points; + for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { + feasible_obj_outer_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->max_value, 0.0) + feasible_obj_inner_bound_pose, 0.0, + object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) .position); } // create obj_polygon from inner/outer bound points tier4_autoware_utils::Polygon2d obj_poly; - for (const auto & bound_point : obj_inner_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); - } - std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); - for (const auto & bound_point : obj_outer_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); - } + const auto add_points_to_obj_poly = [&](const auto & bound_points) { + for (const auto & bound_point : bound_points) { + obj_poly.outer().push_back(tier4_autoware_utils::Point2d(bound_point.x, bound_point.y)); + } + }; + add_points_to_obj_poly(feasible_obj_inner_bound_points); + std::reverse(feasible_obj_outer_bound_points.begin(), feasible_obj_outer_bound_points.end()); + add_points_to_obj_poly(feasible_obj_outer_bound_points); boost::geometry::correct(obj_poly); return obj_poly; diff --git a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..16b4ffbc688ae --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,123 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_dynamic_avoidance_module") + + "/config/dynamic_avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..86dc0ccb61330 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_external_request_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/scene.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package() diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..dfcc4f61d8241 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_lane_change_module/manager.hpp" +#include "route_handler/route_handler.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeRightModuleManager() + : LaneChangeModuleManager( + "external_request_lane_change_right", route_handler::Direction::RIGHT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; + +class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeLeftModuleManager() + + : LaneChangeModuleManager( + "external_request_lane_change_left", route_handler::Direction::LEFT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp similarity index 79% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp rename to planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp index d9c77d6db337e..c23d6f2f3996c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include @@ -35,4 +35,4 @@ class ExternalRequestLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_external_request_lane_change_module/package.xml new file mode 100644 index 0000000000000..16216aa1f71e9 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/package.xml @@ -0,0 +1,39 @@ + + + + behavior_path_external_request_lane_change_module + 0.1.0 + The behavior_path_external_request_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_lane_change_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..f3cc686837c38 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..3cfe7aa51f0f1 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_external_request_lane_change_module/manager.hpp" + +#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/interface.hpp" + +namespace behavior_path_planner +{ + +std::unique_ptr +ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +std::unique_ptr +ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 92% rename from planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp rename to planning/behavior_path_external_request_lane_change_module/src/scene.cpp index 3a304a9b5bb53..913266bf79fa3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" +#include "behavior_path_external_request_lane_change_module/scene.hpp" #include diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..434988cc3ab08 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,128 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, { + planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + }); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_goal_planner_module/README.md similarity index 97% rename from planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md rename to planning/behavior_path_goal_planner_module/README.md index 888d5d9ff9826..f777e49e5fd78 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -91,7 +91,7 @@ Either one is activated when all conditions are met. 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) +![path_goal_refinement](./images/path_goal_refinement.drawio.svg) @@ -203,7 +203,7 @@ The lateral jerk is searched for among the predetermined minimum and maximum val 2. In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) 3. Combine this path with center line of road lane -![shift_parking](../image/shift_parking.drawio.svg) +![shift_parking](./images/shift_parking.drawio.svg) [shift_parking video](https://user-images.githubusercontent.com/39142679/178034101-4dc61a33-bc49-41a0-a9a8-755cce53cbc6.mp4) @@ -234,7 +234,7 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 Generate two forward arc paths. -![arc_forward_parking](../image/arc_forward_parking.drawio.svg) +![arc_forward_parking](./images/arc_forward_parking.drawio.svg) [arc_forward_parking video](https://user-images.githubusercontent.com/39142679/178034128-4754c401-8aff-4745-b69a-4a69ca29ce4b.mp4) @@ -251,7 +251,7 @@ Generate two forward arc paths. Generate two backward arc paths. -![arc_backward_parking](../image/arc_backward_parking.drawio.svg). +![arc_backward_parking](./images/arc_backward_parking.drawio.svg). [arc_backward_parking video](https://user-images.githubusercontent.com/39142679/178034280-4b6754fe-3981-4aee-b5e0-970f34563c6d.mp4) @@ -267,9 +267,9 @@ Generate two backward arc paths. ### freespace parking 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` +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) +![pull_over_freespace_parking_flowchart](./images/pull_over_freespace_parking_flowchart.drawio.svg) Simultaneous execution with `avoidance_module` in the flowchart is under development. @@ -287,4 +287,4 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop | :----------------------- | :--- | :--- | :------------------------------------------------------------------------------------------------------------------- | :------------ | | enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | -See [freespace_planner](../../freespace_planner/README.md) for other parameters. +See [freespace_planner](../freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_planner/image/arc_backward_parking.drawio.svg b/planning/behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/arc_backward_parking.drawio.svg rename to planning/behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg diff --git a/planning/behavior_path_planner/image/arc_forward_parking.drawio.svg b/planning/behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/arc_forward_parking.drawio.svg rename to planning/behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg diff --git a/planning/behavior_path_planner/image/path_goal_refinement.drawio.svg b/planning/behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_goal_refinement.drawio.svg rename to planning/behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg diff --git a/planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg b/planning/behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg rename to planning/behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg diff --git a/planning/behavior_path_planner/image/shift_parking.drawio.svg b/planning/behavior_path_goal_planner_module/images/shift_parking.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/shift_parking.drawio.svg rename to planning/behavior_path_goal_planner_module/images/shift_parking.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index c5ba04ba7f52e..6de1a726d4d4e 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 8100ed7c66128..e90162c958be5 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -21,8 +21,8 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_goal_planner_module/goal_searcher.hpp" #include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 2d9e2c6cf3700..94cf7bc7ff5dd 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ddd7c93998654..fe24d66ddc850 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -17,7 +17,6 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include #include @@ -114,7 +113,7 @@ class PullOverPlannerBase PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } virtual ~PullOverPlannerBase() = default; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index cc8686f4a9fef..bd19dc2e87de0 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 791c35f3afca6..a7aec66f64363 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -16,11 +16,10 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include +#include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include #include #include @@ -33,9 +32,7 @@ #include #include -namespace behavior_path_planner -{ -namespace goal_planner_utils +namespace behavior_path_planner::goal_planner_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; @@ -68,7 +65,6 @@ MarkerArray createGoalCandidatesMarkerArray( MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); -} // namespace goal_planner_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::goal_planner_utils #endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index f028b8aff8b98..cbbe5f585dbe2 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -33,9 +33,8 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( BehaviorModuleOutput output = // use planner previous module reference path getPreviousModuleOutput(); - const PathWithLaneId smoothed_path = - modifyPathForSmoothGoalConnection(*(output.path), planner_data); - output.path = std::make_shared(smoothed_path); + const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(output.path, planner_data); + output.path = smoothed_path; output.reference_path = getPreviousModuleOutput().reference_path; return output; } diff --git a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index c9af9ee369bd5..6d3eb09bf4352 100644 --- a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -15,8 +15,10 @@ #include "behavior_path_goal_planner_module/freespace_pull_over.hpp" #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 31c1d3ef77b0e..b7d1c240d032a 100644 --- a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -15,6 +15,7 @@ #include "behavior_path_goal_planner_module/geometric_pull_over.hpp" #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 6f1b1e7f41571..7bd97b3775bad 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -15,7 +15,7 @@ #include "behavior_path_goal_planner_module/goal_planner_module.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -93,7 +93,7 @@ GoalPlannerModule::GoalPlannerModule( // set selected goal searcher // currently there is only one goal_searcher_type const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info); + vehicle_footprint_ = vehicle_info.createFootprint(); goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_, occupancy_grid_map_); @@ -282,7 +282,7 @@ void GoalPlannerModule::updateData() resetPathCandidate(); resetPathReference(); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); updateOccupancyGrid(); @@ -761,7 +761,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const // because it takes time for the trajectory to be reflected auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); keepStoppedWithCurrentPath(current_path); - output.path = std::make_shared(current_path); + output.path = current_path; } setModifiedGoal(output); @@ -777,14 +777,14 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const { if (prev_data_.found_path || !prev_data_.stop_path) { // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = std::make_shared(generateStopPath()); + output.path = generateStopPath(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = prev_data_.stop_path; + output.path = *prev_data_.stop_path; // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); + stop_pose_ = utils::getFirstStopPoseFromPath(output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } @@ -797,23 +797,22 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); if (stop_path) { - output.path = std::make_shared(*stop_path); + output.path = *stop_path; RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { - output.path = - std::make_shared(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = prev_data_.stop_path_after_approval; + output.path = *prev_data_.stop_path_after_approval; // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); + stop_pose_ = utils::getFirstStopPoseFromPath(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } } @@ -826,7 +825,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); + output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -853,9 +852,9 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const { const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( - *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); } @@ -942,10 +941,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() BehaviorModuleOutput output{}; const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(); output.modified_goal = pull_over_output.modified_goal; - output.path = std::make_shared(generateStopPath()); + output.path = generateStopPath(); + output.reference_path = getPreviousModuleOutput().reference_path; const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); + output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info{}; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -1053,7 +1053,7 @@ void GoalPlannerModule::postProcess() void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) { if (prev_data_.found_path || !prev_data_.stop_path) { - prev_data_.stop_path = output.path; + prev_data_.stop_path = std::make_shared(output.path); } // for the next loop setOutput(). @@ -1085,7 +1085,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) { return; } - auto stop_path = std::make_shared(*output.path); + auto stop_path = std::make_shared(output.path); for (auto & point : stop_path->points) { point.point.longitudinal_velocity_mps = 0.0; } diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index b087827a9c539..bf359be3de18b 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -17,6 +17,7 @@ #include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 23e9c53efae7b..1a1e66f85b403 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,9 +14,12 @@ #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" + #include #include #include +#include #include @@ -30,17 +33,14 @@ #include #include +namespace behavior_path_planner::goal_planner_utils +{ + using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; - -namespace behavior_path_planner -{ -namespace goal_planner_utils -{ - lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance) @@ -214,5 +214,4 @@ bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_h return false; } -} // namespace goal_planner_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..e5bce68ebfa44 --- /dev/null +++ b/planning/behavior_path_lane_change_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/interface.cpp + src/manager.cpp + src/scene.cpp + src/utils/markers.cpp + src/utils/utils.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_lane_change_module/README.md similarity index 96% rename from planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md rename to planning/behavior_path_lane_change_module/README.md index ee74f700c4d42..bffa5201f2882 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -15,7 +15,7 @@ The Lane Change module is activated when lane change is needed and can be safely The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path. -![lane-change-phases](../image/lane_change/lane_change-lane_change_phases.png) +![lane-change-phases](./images/lane_change-lane_change_phases.png) ### Preparation phase @@ -62,7 +62,7 @@ Note that when the `current_velocity` is lower than `minimum_lane_changing_veloc The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. -![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) +![path_samples](./images/lane_change-candidate_path_samples.png) Which path will be chosen will depend on validity and collision check. @@ -155,21 +155,21 @@ stop #### Candidate Path's Safety check -See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md) +See [safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) #### Objects selection and classification First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. -![object lanes](../image/lane_change/lane_objects.drawio.svg) +![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. -![enable collision check at prepare phase](../image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png) +![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. @@ -184,7 +184,7 @@ minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_ve The following figure illustrates when the lane is blocked in multiple lane changes cases. -![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) #### Stopping position when an object exists ahead @@ -195,25 +195,25 @@ The position to be stopped depends on the situation, such as when the lane chang Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_at_terminal_no_block](../image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg) +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) -![stop_at_terminal](../image/lane_change/lane_change-stop_at_terminal.drawio.svg) +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) ##### When the ego vehicle is not near the end of the lane change If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal_no_blocking_object](../image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal](../image/lane_change/lane_change-stop_not_at_terminal.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) ##### When the target lane is far away When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. -![stop_far_from_target_lane](../image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) ### Lane Change When Stuck @@ -292,19 +292,19 @@ The function can be enabled by setting `enable_on_prepare_phase` to `true`. The following image illustrates the cancel process. -![cancel](../image/lane_change/cancel_and_abort/lane_change-cancel.png) +![cancel](./images/lane_change-cancel.png) #### Abort Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. -![abort](../image/lane_change/cancel_and_abort/lane_change-abort.png) +![abort](./images/lane_change-abort.png) #### Stop/Cruise The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. -![stop](../image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png) +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -451,11 +451,11 @@ Then add the marker in `rviz2`. -![debug](../image/lane_change/lane_change-debug-1.png) +![debug](./images/lane_change-debug-1.png) -![debug2](../image/lane_change/lane_change-debug-2.png) +![debug2](./images/lane_change-debug-2.png) -![debug3](../image/lane_change/lane_change-debug-3.png) +![debug3](./images/lane_change-debug-3.png) Available information diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/lane_change/lane_change.param.yaml rename to planning/behavior_path_lane_change_module/config/lane_change.param.yaml diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png b/planning/behavior_path_lane_change_module/images/lane_change-abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-abort.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png b/planning/behavior_path_lane_change_module/images/lane_change-cancel.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png rename to planning/behavior_path_lane_change_module/images/lane_change-cancel.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png b/planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png rename to planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-1.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-1.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-2.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-2.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-3.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-3.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png b/planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png rename to planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png b/planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png rename to planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png rename to planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png b/planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png rename to planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index b7cef007d6d9f..4e741a2b3db2c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -68,11 +67,15 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + bool isRootLaneletToBeUpdated() const override + { + return getCurrentStatus() == ModuleStatus::SUCCESS; + } void updateData() override; + void postProcess() override; + BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -117,15 +120,13 @@ class LaneChangeInterface : public SceneModuleInterface std::unique_ptr module_type_; - bool canTransitSuccessState() override { return false; } - - bool canTransitFailureState() override { return false; } + PathSafetyStatus post_process_safety_status_; - bool canTransitIdleToRunningState() override { return false; } + bool canTransitSuccessState() override; - void resetPathIfAbort(); + bool canTransitFailureState() override; - void resetLaneChangeModule(); + bool canTransitIdleToRunningState() override; void setObjectDebugVisualization() const; @@ -146,4 +147,4 @@ class LaneChangeInterface : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp similarity index 68% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index 024c6685b28c4..b34f691bd2b0d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "route_handler/route_handler.hpp" @@ -23,7 +23,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -72,28 +71,6 @@ class LaneChangeLeftModuleManager : public LaneChangeModuleManager { } }; - -class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeRightModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_right", route_handler::Direction::RIGHT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; - -class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeLeftModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_left", route_handler::Direction::LEFT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp similarity index 94% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 61caf7b2b393f..9c3c371627a78 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" #include #include @@ -71,7 +71,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isApprovedPathSafe() const override; - bool isRequiredStop(const bool is_object_coming_from_rear) const override; + bool isRequiredStop(const bool is_object_coming_from_rear) override; bool isNearEndOfCurrentLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -180,4 +180,4 @@ class NormalLaneChange : public LaneChangeBase double stop_time_{0.0}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index be49ec5b2e740..451899fbf27e6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -11,12 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -90,7 +90,7 @@ class LaneChangeBase virtual bool isEgoOnPreparePhase() const = 0; - virtual bool isRequiredStop(const bool is_object_coming_from_rear) const = 0; + virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; virtual PathSafetyStatus isApprovedPathSafe() const = 0; @@ -107,14 +107,13 @@ class LaneChangeBase virtual bool specialExpiredCheck() const { return false; } virtual void setPreviousModulePaths( - const std::shared_ptr & prev_module_reference_path, - const std::shared_ptr & prev_module_path) + const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path) { - if (prev_module_reference_path) { - prev_module_reference_path_ = *prev_module_reference_path; + if (!prev_module_reference_path.points.empty()) { + prev_module_reference_path_ = prev_module_reference_path; } - if (prev_module_path) { - prev_module_path_ = *prev_module_path; + if (!prev_module_path.points.empty()) { + prev_module_path_ = prev_module_path; } }; @@ -265,8 +264,8 @@ class LaneChangeBase mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index debeef5ec573f..dd7760d31eaa7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -211,4 +211,4 @@ struct PathSafetyStatus }; } // namespace behavior_path_planner::data::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 96f0851ed33c0..d6deecd4f46fa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include + #include -#include #include namespace marker_utils::lane_change_markers @@ -38,4 +38,4 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index c98f62c47860c..9e42b49635b82 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -50,4 +50,4 @@ struct LaneChangeStatus }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index f99ed043eb5dd..67506326d92aa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 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. @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "rclcpp/logger.hpp" #include @@ -207,5 +208,17 @@ lanelet::ConstLanelets generateExpandedLanelets( const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, const double right_offset); +/** + * @brief Retrieves a logger instance for a specific lane change type. + * + * This function provides a specialized logger for different types of lane change. + * + * @param type A string representing the type of lane change operation. This could be + * a specific maneuver or condition related to lane changing, such as + * 'avoidance_by_lane_change', 'normal', 'external_request'. + * + * @return rclcpp::Logger The logger instance configured for the specified lane change type. + */ +rclcpp::Logger getLogger(const std::string & type); } // namespace behavior_path_planner::utils::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/package.xml b/planning/behavior_path_lane_change_module/package.xml new file mode 100644 index 0000000000000..fb4bef9535a48 --- /dev/null +++ b/planning/behavior_path_lane_change_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_lane_change_module + 0.1.0 + The behavior_path_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml similarity index 76% rename from planning/behavior_path_planner/plugins.xml rename to planning/behavior_path_lane_change_module/plugins.xml index 2028b0d571e68..a598bc8176938 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -1,7 +1,6 @@ - - - + - + + diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp similarity index 77% rename from planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp rename to planning/behavior_path_lane_change_module/src/interface.cpp index 101dd43919361..448f83ecaf15e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/markers.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" @@ -26,7 +26,6 @@ #include #include #include -#include namespace behavior_path_planner { @@ -45,6 +44,7 @@ LaneChangeInterface::LaneChangeInterface( prev_approved_path_{std::make_unique()} { steering_factor_interface_ptr_ = std::make_unique(&node, name); + logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } void LaneChangeInterface::processOnEntry() @@ -64,7 +64,7 @@ void LaneChangeInterface::processOnExit() bool LaneChangeInterface::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } @@ -76,112 +76,6 @@ bool LaneChangeInterface::isExecutionReady() const return module_type_->isSafe(); } -ModuleStatus LaneChangeInterface::updateState() -{ - auto log_warn_throttled = [&](const std::string & message) -> void { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, message); - }; - - if (module_type_->specialExpiredCheck()) { - log_warn_throttled("expired check."); - if (isWaitingApproval()) { - return ModuleStatus::SUCCESS; - } - } - - if (!isActivated() || isWaitingApproval()) { - log_warn_throttled("Is idling."); - return ModuleStatus::IDLE; - } - - if (!module_type_->isValidPath()) { - log_warn_throttled("Is invalid path."); - return ModuleStatus::SUCCESS; - } - - if (module_type_->isAbortState()) { - log_warn_throttled("Ego is in the process of aborting lane change."); - return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING; - } - - if (module_type_->hasFinishedLaneChange()) { - log_warn_throttled("Completed lane change."); - return ModuleStatus::SUCCESS; - } - - if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Ego stopped at traffic light. Canceling lane change"); - module_type_->toCancelState(); - return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; - } - - const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); - - setObjectDebugVisualization(); - if (is_safe) { - log_warn_throttled("Lane change path is safe."); - module_type_->toNormalState(); - return ModuleStatus::RUNNING; - } - - const auto change_state_if_stop_required = [&]() -> void { - if (module_type_->isRequiredStop(is_object_coming_from_rear)) { - module_type_->toStopState(); - } else { - module_type_->toNormalState(); - } - }; - - if (!module_type_->isCancelEnabled()) { - log_warn_throttled( - "Lane change path is unsafe but cancel was not enabled. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - if (!module_type_->isAbleToReturnCurrentLane()) { - log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane; - const auto status = module_type_->getLaneChangeStatus(); - if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) { - log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { - log_warn_throttled("Lane change path is unsafe. Cancel lane change."); - module_type_->toCancelState(); - return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; - } - - if (!module_type_->isAbortEnabled()) { - log_warn_throttled( - "Lane change path is unsafe but abort was not enabled. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - const auto found_abort_path = module_type_->calcAbortPath(); - if (!found_abort_path) { - log_warn_throttled( - "Lane change path is unsafe but not found abort path. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - log_warn_throttled("Lane change path is unsafe. Abort lane change."); - module_type_->toAbortState(); - return ModuleStatus::RUNNING; -} - void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( @@ -190,6 +84,11 @@ void LaneChangeInterface::updateData() module_type_->resetStopPose(); } +void LaneChangeInterface::postProcess() +{ + post_process_safety_status_ = module_type_->isApprovedPathSafe(); +} + BehaviorModuleOutput LaneChangeInterface::plan() { resetPathCandidate(); @@ -202,8 +101,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); - path_reference_ = output.reference_path; - *prev_approved_path_ = *getPreviousModuleOutput().path; + path_reference_ = std::make_shared(output.reference_path); + *prev_approved_path_ = getPreviousModuleOutput().path; stop_pose_ = module_type_->getStopPose(); @@ -220,12 +119,12 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = *getPreviousModuleOutput().path; + *prev_approved_path_ = getPreviousModuleOutput().path; module_type_->insertStopPoint( module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = std::make_shared(*prev_approved_path_); + out.path = *prev_approved_path_; out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; @@ -241,9 +140,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() } // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); + out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); stop_pose_ = module_type_->getStopPose(); @@ -288,6 +187,135 @@ void LaneChangeInterface::setData(const std::shared_ptr & dat module_type_->setData(data); } +bool LaneChangeInterface::canTransitSuccessState() +{ + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + if (module_type_->specialExpiredCheck() && isWaitingApproval()) { + log_debug_throttled("Run specialExpiredCheck."); + if (isWaitingApproval()) { + return true; + } + } + + if (!module_type_->isValidPath()) { + log_debug_throttled("Has no valid path."); + return true; + } + + if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has completed."); + return true; + } + + if (module_type_->hasFinishedLaneChange()) { + log_debug_throttled("Lane change process has completed."); + return true; + } + + log_debug_throttled("Lane changing process is ongoing"); + return false; +} + +bool LaneChangeInterface::canTransitFailureState() +{ + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + log_debug_throttled(__func__); + + if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has on going."); + return false; + } + + if (isWaitingApproval()) { + log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); + return false; + } + + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { + if (module_type_->isStoppedAtRedTrafficLight()) { + log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); + module_type_->toCancelState(); + return true; + } + + if (post_process_safety_status_.is_safe) { + log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); + return false; + } + + if (module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + return true; + } + } + + if (post_process_safety_status_.is_safe) { + log_debug_throttled("Can't transit to failure state. Ego is lane changing, and it's safe."); + return false; + } + + if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { + log_debug_throttled("Module require stopping"); + } + + if (!module_type_->isCancelEnabled()) { + log_debug_throttled( + "Lane change path is unsafe but cancel was not enabled. Continue lane change."); + return false; + } + + if (!module_type_->isAbortEnabled()) { + log_debug_throttled( + "Lane change path is unsafe but abort was not enabled. Continue lane change."); + return false; + } + + if (!module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); + return false; + } + + const auto found_abort_path = module_type_->calcAbortPath(); + if (!found_abort_path) { + log_debug_throttled( + "Lane change path is unsafe but abort path not found. Continue lane change."); + return false; + } + + log_debug_throttled("Lane change path is unsafe. Abort lane change."); + module_type_->toAbortState(); + return false; +} + +bool LaneChangeInterface::canTransitIdleToRunningState() +{ + setObjectDebugVisualization(); + + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + log_debug_throttled(__func__); + + if (!isActivated() || isWaitingApproval()) { + if (module_type_->specialRequiredCheck()) { + return true; + } + log_debug_throttled("Module is idling."); + return false; + } + + log_debug_throttled("Can lane change safely. Executing lane change."); + module_type_->toNormalState(); + return true; +} + void LaneChangeInterface::setObjectDebugVisualization() const { debug_marker_.markers.clear(); @@ -336,7 +364,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() return marker; } - if (isWaitingApproval() || current_state_ != ModuleStatus::RUNNING) { + if (isWaitingApproval() || getCurrentStatus() != ModuleStatus::RUNNING) { return marker; } const auto & start_pose = module_type_->getLaneChangePath().info.lane_changing_start; @@ -367,9 +395,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); const auto start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_position, status.lane_change_path.info.shift_line.start.position); + output.path.points, current_position, status.lane_change_path.info.shift_line.start.position); const auto finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_position, status.lane_change_path.info.shift_line.end.position); + output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp similarity index 95% rename from planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp rename to planning/behavior_path_lane_change_module/src/manager.cpp index 658ffb06c25fe..e4405a31a360d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -262,16 +263,10 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { - if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); - } return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, direction_)); + std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) @@ -298,9 +293,3 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( behavior_path_planner::LaneChangeLeftModuleManager, behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp similarity index 98% rename from planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp rename to planning/behavior_path_lane_change_module/src/scene.cpp index ebb41ae63dcbe..c7ddd8fe63636 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -145,38 +145,38 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() BehaviorModuleOutput output; if (isAbortState() && abort_path_) { - output.path = std::make_shared(abort_path_->path); - output.reference_path = std::make_shared(prev_module_reference_path_); + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; output.turn_signal_info = prev_turn_signal_info_; - insertStopPoint(status_.current_lanes, *output.path); + insertStopPoint(status_.current_lanes, output.path); } else { - output.path = std::make_shared(getLaneChangePath().path); + output.path = getLaneChangePath().path; const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::combinePath(*output.path, *found_extended_path); + output.path = utils::combinePath(output.path, *found_extended_path); } - output.reference_path = std::make_shared(getReferencePath()); + output.reference_path = getReferencePath(); output.turn_signal_info = updateOutputTurnSignal(); if (isStopState()) { const auto current_velocity = getEgoVelocity(); const auto current_dist = calcSignedArcLength( - output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, *output.path); + insertStopPoint(status_.target_lanes, output.path); } } extendOutputDrivableArea(output); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( - *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -189,7 +189,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *getRouteHandler(), status_.current_lanes, status_.target_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); @@ -997,12 +997,7 @@ PathWithLaneId NormalLaneChange::getTargetSegment( std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); }); - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getTargetSegment"), - "start: %f, end: %f", s_start, s_end); + RCLCPP_DEBUG(logger_, "in %s start: %f, end: %f", __func__, s_start, s_end); PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); for (auto & point : target_segment.points) { @@ -1531,11 +1526,17 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const return true; } -bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const +bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) { const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - return isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && - isAbleToStopSafely() && is_object_coming_from_rear; + if ( + isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isAbleToStopSafely() && is_object_coming_from_rear) { + current_lane_change_state_ = LaneChangeStates::Stop; + return true; + } + current_lane_change_state_ = LaneChangeStates::Normal; + return false; } bool NormalLaneChange::calcAbortPath() diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp similarity index 98% rename from planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp rename to planning/behavior_path_lane_change_module/src/utils/markers.cpp index c2a640f989b50..719acba405a68 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,7 +16,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/lane_change/utils.cpp rename to planning/behavior_path_lane_change_module/src/utils/utils.cpp index 8ba820231d7cc..df73990b770a3 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -244,7 +244,9 @@ lanelet::ConstLanelets getTargetNeighborLanes( for (const auto & current_lane : current_lanes) { if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { neighbor_lanes.push_back(current_lane); } } else { @@ -782,7 +784,9 @@ std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { return route_handler.getLaneChangeTarget(current_lanes, direction); } @@ -1159,4 +1163,9 @@ lanelet::ConstLanelets generateExpandedLanelets( const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); } + +rclcpp::Logger getLogger(const std::string & type) +{ + return rclcpp::get_logger("lane_change").get_child(type); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..82f721411d5a4 --- /dev/null +++ b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp similarity index 95% rename from planning/behavior_path_planner/test/test_lane_change_utils.cpp rename to planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 477af32893b0b..1f90114df8045 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,8 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include #include diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 56456b08aac1e..54b4bf122da00 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,20 +7,9 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) - ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp - src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/lane_change/interface.cpp - src/scene_module/lane_change/normal.cpp - src/scene_module/lane_change/external_request.cpp - src/scene_module/lane_change/manager.cpp - src/utils/lane_change/utils.cpp - src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/lane_change/debug.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC @@ -46,14 +35,6 @@ if(BUILD_TESTING) ${PROJECT_NAME}_lib ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module - test/test_lane_change_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module - ${PROJECT_NAME}_lib - ) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface test/test_${PROJECT_NAME}_node_interface.cpp ) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 884767315ac12..03f8c8d2a5696 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -24,17 +24,17 @@ Essentially, the module has three primary responsibilities: Behavior Path Planner has following scene modules -| Name | Description | Details | -| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | -| Dynamic Avoidance | WIP | LINK | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_by_lane_change_design.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | -| External Lane Change | WIP | LINK | -| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | -| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_start_planner_design.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | +| Name | Description | Details | +| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | +| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | +| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | !!! Note @@ -139,7 +139,6 @@ Enabling and disabling the modules in the behavior path planner is primarily man The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. -- `use_experimental_lane_change_function`: Set to `true` to enable experimental features in the lane change module. !!! note @@ -169,7 +168,7 @@ The shifted path generation logic enables the behavior path planner to dynamical !!! note - If you're a math lover, refer to [Path Generation Design](./docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. + If you're a math lover, refer to [Path Generation Design](../behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. ## Collision Assessment / Safety check @@ -186,7 +185,7 @@ However, the module does have a limitation concerning the yaw angle of each poin !!! note - For further reading on the collision assessment method, please refer to [Safety check utils](./docs/behavior_path_planner_safety_check.md) + For further reading on the collision assessment method, please refer to [Safety check utils](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) ## Generating Drivable Area @@ -209,7 +208,7 @@ Static drivable area expansion operates under assumptions about the correct arra !!! note - Further details can is provided in [Drivable Area Design](./docs/behavior_path_planner_drivable_area_design.md). + Further details can is provided in [Drivable Area Design](../behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). ### Dynamic Drivable Area Logic @@ -229,7 +228,7 @@ The `TurnIndicatorsCommand` message structure has a command field that can take !!! note - For more in-depth information, refer to [Turn Signal Design](./docs/behavior_path_planner_turn_signal_design.md) document. + For more in-depth information, refer to [Turn Signal Design](../behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md) document. ## Rerouting diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index b4d323de45cde..0c5dbc082c9b9 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,8 +25,6 @@ input_path_interval: 2.0 output_path_interval: 2.0 - visualize_maximum_drivable_area: true - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index c0b6f259c7726..bb897db393b65 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -13,7 +13,7 @@ smoothing: curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length - extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied + arc_length_range: 2.0 # [m] arc length range where an expansion distance is initially applied ego: extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase extra_front_overhang: 0.5 # [m] extra length to add to the front overhang diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md deleted file mode 100644 index 056e41781e262..0000000000000 --- a/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md +++ /dev/null @@ -1,143 +0,0 @@ -# Drivable Area design - -Drivable Area represents the area where ego vehicle can pass. - -## Purpose / Role - -In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. - -## Assumption - -Our drivable area has several assumptions. - -- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. - -- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). - -- Both left and right bounds should cover the front of the path and the end of the path. - -## Limitations - -Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. - -## Parameters for drivable area generation - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------- | :--- | :----------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------ | -| enabled | [-] | boolean | whether to dynamically the drivable area using the ego footprint | false | -| ego.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.0 | -| ego.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.0 | -| ego.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.0 | -| ego.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.0 | -| dynamic_objects.avoid | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true | -| dynamic_objects.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 | -| dynamic_objects.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | -| dynamic_objects.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 | -| dynamic_objects.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | -| max_distance | [m] | double | maximum distance by which the drivable area can be expended. A value of 0.0 means no maximum distance. | 0.0 | -| expansion.method | [-] | string | method to use for the expansion: "polygon" will expand the drivable area around the ego footprint polygons; "lanelet" will expand to the whole lanelets overlapped by the ego footprints | "polygon" | -| expansion.max_arc_path_length | [m] | double | maximum length along the path where the ego footprint is projected | 50.0 | -| expansion.extra_arc_length | [m] | double | extra arc length (relative to the path) around ego footprints where the drivable area is expanded | 0.5 | -| expansion.avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | [guard_rail, road_border] | -| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 | -| avoid_linestring.compensate.enable | [-] | bool | if true, when the expansion is blocked by a linestring on one side of the path, we try to compensate and expand on the other side | true | -| avoid_linestring.compensate.extra_distance | [m] | double | extra distance added to the expansion when compensating | 3.0 | - -The following parameters are defined for each module. Please refer to the `config/drivable_area_expansion.yaml` file. - -| Name | Unit | Type | Description | Default value | -| :------------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ | -| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | -| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | -| drivable_area_types_to_skip | [-] | string | linestring types (as defined in the lanelet map) that will not be expanded | road_border | - -## Inner-workings / Algorithms - -This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). - -### Drivable Lanes Generation - -Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Goal Planner`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. - -```cpp -struct DrivalbleLanes -{ - lanelet::ConstLanelet right_lanelet; // right most lane - lanelet::ConstLanelet left_lanelet; // left most lane - lanelet::ConstLanelets middle_lanelets; // middle lanes -}; -``` - -The image of the sorted drivable lanes is depicted in the following picture. - -![sorted_lanes](../image/drivable_area/sorted_lanes.drawio.svg) - -Note that, the order of drivable lanes become - -```cpp -drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} -``` - -### Drivable Area Generation - -In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as - -```cpp -std::vector left_bound; -std::vector right_bound; -``` - -and each point of right bound and left bound has a position in the absolute coordinate system. - -![drivable_lines](../image/drivable_area/drivable_lines.drawio.svg) - -### Drivable Area Expansion - -#### Static Expansion - -Each module can statically expand the left and right bounds of the target lanes by the parameter defined values. -This enables large vehicles to pass narrow curve. The image of this process can be described as - -![expanded_lanes](../image/drivable_area/expanded_lanes.drawio.svg) - -Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. - -#### Dynamic Expansion - -The drivable area can also be expanded dynamically by considering the ego vehicle footprint projected on each path point. -This expansion can be summarized with the following steps: - -1. Build the ego path footprint. -2. Build the dynamic objects' predicted footprints (optional). -3. Build "uncrossable" lines. -4. Remove the footprints from step 2 and the lines from step 3 from the ego path footprint from step 1. -5. Expand the drivable area with the result of step 4. - -| | | -| :------------------------------- | :--------------------------------------------------------------------------------------------------- | -| Inputs | ![drivable_area_expansion_inputs](../image/drivable_area/drivable_area_expansion_inputs.png) | -| Footprints and uncrossable lines | ![drivable_area_expansion_footprints](../image/drivable_area/drivable_area_expansion_footprints.png) | -| Expanded drivable area | ![drivable_area_expansion_result](../image/drivable_area/drivable_area_expansion_result.png) | - -Please note that the dynamic expansion can only increase the size of the drivable area and cannot remove any part from the original drivable area. - -### Visualizing maximum drivable area (Debug) - -Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. - -For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. - -To debug the issue, the maximum drivable area boundary can be visualized. - -![drivable_area_boundary_marker1](../image/drivable_area/drivable_area_boundary_marker_example1.png) - -![drivable_area_boundary_marker2](../image/drivable_area/drivable_area_boundary_marker_example2.png) - -The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` - -#### Expansion with hatched road markings area - -If the hatched road markings area is defined in the lanelet map, the area can be used as a drivable area. -Since the area is expressed as a polygon format of Lanelet2, several steps are required for correct expansion. - -![hatched_road_markings_drivable_area_expansion](../image/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg) diff --git a/planning/behavior_path_planner/image/avoid_fig_multi_case.png b/planning/behavior_path_planner/image/avoid_fig_multi_case.png deleted file mode 100644 index 014806fa7a4b6..0000000000000 Binary files a/planning/behavior_path_planner/image/avoid_fig_multi_case.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/avoid_fig_single_case.png b/planning/behavior_path_planner/image/avoid_fig_single_case.png deleted file mode 100644 index 7b50a286dca99..0000000000000 Binary files a/planning/behavior_path_planner/image/avoid_fig_single_case.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_footprints.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_footprints.png deleted file mode 100644 index 0ec8bf7f5cf88..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_footprints.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_inputs.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_inputs.png deleted file mode 100644 index d2ba94a5b21aa..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_inputs.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_result.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_result.png deleted file mode 100644 index 373cf4899086d..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_result.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area_plugin.png b/planning/behavior_path_planner/image/drivable_area_plugin.png deleted file mode 100644 index d63e7b71934c3..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area_plugin.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change_fig1.png b/planning/behavior_path_planner/image/lane_change_fig1.png deleted file mode 100644 index 74fbf25f86394..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change_fig1.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change_fig2.png b/planning/behavior_path_planner/image/lane_change_fig2.png deleted file mode 100644 index 5a87e43733520..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change_fig2.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change_fig3.png b/planning/behavior_path_planner/image/lane_change_fig3.png deleted file mode 100644 index 9e1791a05d098..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change_fig3.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/minimum_lane_change_distance.png b/planning/behavior_path_planner/image/minimum_lane_change_distance.png deleted file mode 100644 index cdde3be0eff38..0000000000000 Binary files a/planning/behavior_path_planner/image/minimum_lane_change_distance.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/path_shifter_old.png b/planning/behavior_path_planner/image/path_shifter_old.png deleted file mode 100644 index 91542fdd0d042..0000000000000 Binary files a/planning/behavior_path_planner/image/path_shifter_old.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/shift_length_computation.png b/planning/behavior_path_planner/image/shift_length_computation.png deleted file mode 100644 index 5c6f364ececd0..0000000000000 Binary files a/planning/behavior_path_planner/image/shift_length_computation.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/side_shift_fig1.png b/planning/behavior_path_planner/image/side_shift_fig1.png deleted file mode 100644 index e851e84ffb555..0000000000000 Binary files a/planning/behavior_path_planner/image/side_shift_fig1.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/start_from_road_lane.drawio.svg b/planning/behavior_path_planner/image/start_from_road_lane.drawio.svg deleted file mode 100644 index 7a89cc2183acf..0000000000000 --- a/planning/behavior_path_planner/image/start_from_road_lane.drawio.svg +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - 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 c47dc70213d1a..5d40f2a8614ed 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 @@ -166,7 +166,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & output); // debug - rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; 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 7ceafb6f428de..f6f95ae3b5e82 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 @@ -286,6 +286,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishSteeringFactor(); + module_ptr->publishObjectsOfInterestMarker(); processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); @@ -433,7 +435,16 @@ class PlannerManager const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output); - std::string getNames(const std::vector & modules) const; + /** + * @brief run keep last approved modules + * @param planner data. + * @param previous module output. + * @return planning result. + */ + BehaviorModuleOutput runKeepLastModules( + const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const; + + static std::string getNames(const std::vector & modules); std::optional root_lanelet_{std::nullopt}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp deleted file mode 100644 index 98d52b79e2edf..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2021-2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ - -namespace behavior_path_planner -{ - -struct LaneFollowingParameters -{ - double lane_change_prepare_duration{0.0}; - - // finding closest lanelet - double distance_threshold{0.0}; - double yaw_threshold{0.0}; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 0ca7badd0b3f4..2bc73325f96d5 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -56,7 +56,6 @@ pluginlib rclcpp rclcpp_components - route_handler sensor_msgs signal_processing tf2 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 46057031afd8a..9b3fbedc37203 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include #include @@ -73,11 +74,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); - if (planner_data_->parameters.visualize_maximum_drivable_area) { - debug_maximum_drivable_area_publisher_ = - create_publisher("~/maximum_drivable_area", 1); - } - debug_turn_signal_info_publisher_ = create_publisher("~/debug/turn_signal_info", 1); bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -135,6 +131,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } planner_manager_->launchScenePlugin(*this, name); } @@ -260,7 +260,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.enable_cog_on_centerline = declare_parameter("enable_cog_on_centerline"); p.input_path_interval = declare_parameter("input_path_interval"); p.output_path_interval = declare_parameter("output_path_interval"); - p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -443,12 +442,6 @@ void BehaviorPathPlannerNode::run() planner_data_->prev_route_id = planner_data_->route_handler->getRouteUuid(); - if (planner_data_->parameters.visualize_maximum_drivable_area) { - const auto maximum_drivable_area = marker_utils::createFurthestLineStringMarkerArray( - utils::getMaximumDrivableArea(planner_data_)); - debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); - } - lk_pd.unlock(); // release planner_data_ planner_manager_->print(); @@ -498,16 +491,10 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { - if (intersection_flag) { - return SteeringFactor::TURNING; - } - return SteeringFactor::TRYING; - }); steering_factor_interface_ptr_->updateSteeringFactor( {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, ""); + PlanningBehavior::INTERSECTION, steering_factor_direction, SteeringFactor::TURNING, ""); } else { steering_factor_interface_ptr_->clearSteeringFactors(); } @@ -714,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = utils::toPath(*path_candidate_ptr); + output = motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); @@ -734,7 +722,8 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( { // TODO(Horibe) do some error handling when path is not available. - auto path = output.path ? output.path : planner_data->prev_output_path; + auto path = !output.path.points.empty() ? std::make_shared(output.path) + : planner_data->prev_output_path; path->header = planner_data->route_handler->getRouteHeader(); path->header.stamp = this->now(); @@ -924,8 +913,8 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); updateParam( - parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM, + planner_data_->drivable_area_expansion_parameters.arc_length_range); updateParam( parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, planner_data_->drivable_area_expansion_parameters.print_runtime); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c867bffc8e9bd..262de4764fcfe 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -130,7 +130,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da /** * STEP1: get approved modules' output */ - const auto approved_modules_output = runApprovedModules(data); + auto approved_modules_output = runApprovedModules(data); /** * STEP2: check modules that need to be launched @@ -141,8 +141,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da * STEP3: if there is no module that need to be launched, return approved modules' output */ if (request_modules.empty()) { + const auto output = runKeepLastModules(data, approved_modules_output); processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + return output; } /** @@ -150,24 +151,32 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da */ const auto [highest_priority_module, candidate_modules_output] = runRequestModules(request_modules, data, approved_modules_output); + + /** + * STEP5: run keep last approved modules after running candidate modules. + * NOTE: if no candidate module is launched, approved_modules_output used as input for keep + * last modules and return the result immediately. + */ + const auto output = runKeepLastModules( + data, highest_priority_module ? candidate_modules_output : approved_modules_output); if (!highest_priority_module) { processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + return output; } /** - * STEP5: if the candidate module's modification is NOT approved yet, return the result. + * STEP6: if the candidate module's modification is NOT approved yet, return the result. * NOTE: the result is output of the candidate module, but the output path don't contains path * shape modification that needs approval. On the other hand, it could include velocity * profile modification. */ if (highest_priority_module->isWaitingApproval()) { processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + return output; } /** - * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ + * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_ */ addApprovedModule(highest_priority_module); clearCandidateModules(); @@ -178,7 +187,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", max_iteration_num_); processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + return output; } } @@ -198,7 +207,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da void PlannerManager::generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const { - if (!output.path || output.path->points.empty()) { + if (output.path.points.empty()) { RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!"); return; } @@ -206,20 +215,20 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); + const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path.points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (epsilon < std::abs(di.drivable_margin)) { // for single free space pull over utils::generateDrivableArea( - *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); + output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, + output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, is_driving_forward); } else { - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); const auto & dp = data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -228,19 +237,19 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( - *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, + output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, is_driving_forward); } // extract obstacles from drivable area - utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles); + utils::extractObstaclesFromDrivableArea(output.path, di.obstacles); } std::vector PlannerManager::getRequestModules( const BehaviorModuleOutput & previous_module_output) const { - if (!previous_module_output.path) { + if (previous_module_output.path.points.empty()) { RCLCPP_ERROR_STREAM( logger_, "Current module output is null. Skip candidate module check." << "\n - Approved module list: " << getNames(approved_module_ptrs_) @@ -272,22 +281,22 @@ std::vector PlannerManager::getRequestModules( // if there exists at least one approved module that is simultaneous but not always // executable. (only modules that are either always executable or simultaneous executable can // be added) - conditions.push_back( - {[&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + conditions.emplace_back( + [&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }); // Condition 3: do not add modules that are not always executable if there exists // at least one approved module that is neither always nor simultaneous executable. // (only modules that are always executable can be added) - conditions.push_back( - {[&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return false; }}); + conditions.emplace_back( + [&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }); bool skip_module = false; for (const auto & condition : conditions) { @@ -394,6 +403,19 @@ std::vector PlannerManager::getRequestModules( return request_modules; } +BehaviorModuleOutput PlannerManager::runKeepLastModules( + const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const +{ + auto output = previous_output; + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + if (getManager(m)->isKeepLast()) { + output = run(m, data, output); + } + }); + + return output; +} + BehaviorModuleOutput PlannerManager::getReferencePath( const std::shared_ptr & data) const { @@ -484,22 +506,22 @@ std::pair PlannerManager::runRequestModule // Condition 3: Only modules that are always executable can be added // if there exists at least one executable module that is neither always nor simultaneous // executable. - conditions.push_back( - {[this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return false; }}); + conditions.emplace_back( + [this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }); // Condition 2: Only modules that are either always executable or simultaneous executable can be // added if there exists at least one executable module that is simultaneous but not always // executable. - conditions.push_back( - {[this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + conditions.emplace_back( + [this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }); for (const auto & condition : conditions) { const auto & find_block_module = condition.first; @@ -652,11 +674,13 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(), output); + if (!getManager(m)->isKeepLast()) { + output = run(m, data, output); + results.emplace(m->name(), output); + } }); /** @@ -751,11 +775,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname().find("lane_change") != std::string::npos; }); - - if (success_lane_change) { + if (std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) { + return m->isRootLaneletToBeUpdated(); + })) { root_lanelet_ = updateRootLanelet(data); } @@ -837,9 +859,11 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) // when lane change module is running, don't update root lanelet. const bool is_lane_change_running = std::invoke([&]() { - const auto lane_change_itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); + const auto lane_change_itr = + std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { + return m->name().find("lane_change") != std::string::npos || + m->name().find("avoidance_by_lc") != std::string::npos; + }); return lane_change_itr != approved_module_ptrs_.end(); }); if (is_lane_change_running) { @@ -933,8 +957,9 @@ void PlannerManager::print() const string_stream << "\n" << std::fixed << std::setprecision(1); string_stream << "processing time : "; for (const auto & t : processing_time_) { - string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first - << ":" << std::setw(4) << std::right << t.second << "ms]\n" + string_stream << std::right << "[" << std::setw(static_cast(max_string_num) + 1) + << std::left << t.first << ":" << std::setw(4) << std::right << t.second + << "ms]\n" << std::setw(21); } @@ -962,7 +987,7 @@ std::shared_ptr PlannerManager::getDebugMsg() return debug_msg_ptr_; } -std::string PlannerManager::getNames(const std::vector & modules) const +std::string PlannerManager::getNames(const std::vector & modules) { std::stringstream ss; for (const auto & m : modules) { diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index bab77f9141a00..7ba934e873a8d 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include @@ -49,26 +50,18 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", - behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml"}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner_common/CMakeLists.txt index 5dbf81280f9b1..dbbd7c7cedc67 100644 --- a/planning/behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner_common/CMakeLists.txt @@ -4,7 +4,6 @@ project(behavior_path_planner_common) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) ament_auto_add_library(${PROJECT_NAME} SHARED @@ -23,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/drivable_area_expansion/footprints.cpp src/utils/parking_departure/geometric_parallel_parking.cpp src/utils/parking_departure/utils.cpp + src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp ) @@ -30,10 +30,6 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) -target_link_libraries(${PROJECT_NAME} - ${OpenCV_LIBRARIES} -) - if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities test/test_drivable_area_expansion.cpp diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md new file mode 100644 index 0000000000000..015ffcdfd8f3b --- /dev/null +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md @@ -0,0 +1,178 @@ +# Drivable Area design + +Drivable Area represents the area where ego vehicle can pass. + +## Purpose / Role + +In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. + +## Assumption + +Our drivable area has several assumptions. + +- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. + +- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). + +- Both left and right bounds should cover the front of the path and the end of the path. + +## Limitations + +Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. + +## Parameters for drivable area generation + +### Static expansion + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ | +| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | +| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | +| drivable_area_types_to_skip | [-] | string | linestring types (as defined in the lanelet map) that will not be expanded | road_border | + +### Dynamic expansion + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | :---- | :----------- | :------------------------------------------------------------------------------------------------------ | :--------------------------- | +| enabled | [-] | boolean | if true, dynamically expand the drivable area based on the path curvature | true | +| print_runtime | [-] | boolean | if true, runtime is logged by the node | true | +| max_expansion_distance | [m] | double | maximum distance by which the original drivable area can be expanded (no limit if set to 0) | 0.0 | +| smoothing.curvature_average_window | [-] | int | window size used for smoothing the curvatures using a moving window average | 3 | +| smoothing.max_bound_rate | [m/m] | double | maximum rate of change of the bound lateral distance over its arc length | 1.0 | +| smoothing.arc_length_range | [m] | double | arc length range where an expansion distance is initially applied | 2.0 | +| ego.extra_wheel_base | [m] | double | extra ego wheelbase | 0.0 | +| ego.extra_front_overhang | [m] | double | extra ego overhang | 0.5 | +| ego.extra_width | [m] | double | extra ego width | 1.0 | +| dynamic_objects.avoid | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true | +| dynamic_objects.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 | +| dynamic_objects.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | +| dynamic_objects.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 | +| dynamic_objects.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | +| path_preprocessing.max_arc_length | [m] | double | maximum arc length along the path where the ego footprint is projected (0.0 means no limit) | 100.0 | +| path_preprocessing.resample_interval | [m] | double | fixed interval between resampled path points (0.0 means path points are directly used) | 2.0 | +| path_preprocessing.reuse_max_deviation | [m] | double | if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused | 0.5 | +| avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | ["road_border", "curbstone"] | +| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 | + +## Inner-workings / Algorithms + +This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). + +### Drivable Lanes Generation + +Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Goal Planner`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. + +```cpp +struct DrivalbleLanes +{ + lanelet::ConstLanelet right_lanelet; // right most lane + lanelet::ConstLanelet left_lanelet; // left most lane + lanelet::ConstLanelets middle_lanelets; // middle lanes +}; +``` + +The image of the sorted drivable lanes is depicted in the following picture. + +![sorted_lanes](../images/drivable_area/sorted_lanes.drawio.svg) + +Note that, the order of drivable lanes become + +```cpp +drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} +``` + +### Drivable Area Generation + +In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as + +```cpp +std::vector left_bound; +std::vector right_bound; +``` + +and each point of right bound and left bound has a position in the absolute coordinate system. + +![drivable_lines](../images/drivable_area/drivable_lines.drawio.svg) + +### Drivable Area Expansion + +#### Static Expansion + +Each module can statically expand the left and right bounds of the target lanes by the parameter defined values. +This enables large vehicles to pass narrow curve. The image of this process can be described as + +![expanded_lanes](../images/drivable_area/expanded_lanes.drawio.svg) + +Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. + +#### Dynamic Expansion + +The drivable area can also be expanded dynamically based on a minimum width calculated from the path curvature and the ego vehicle's properties. +If static expansion is also enabled, the dynamic expansion will be done after the static expansion such that both expansions are applied. + +| Without dynamic expansion | With dynamic expansion | +| --------------------------------------------------------------------------- | ------------------------------------------------------------------------- | +| ![dynamic_expansion_off](../images/drivable_area/dynamic_expansion_off.png) | ![dynamic_expansion_on](../images/drivable_area/dynamic_expansion_on.png) | + +Next we detail the algorithm used to expand the drivable area bounds. + +##### 1 Calculate and smooth the path curvature + +To avoid sudden changes of the dynamically expanded drivable area, we first try to reuse as much of the previous path and its calculated curvatures as possible. +Previous path points and curvatures are reused up to the first previous path point that deviates from the new path by more than the `reuse_max_deviation` parameter. +At this stage, the path is also resampled according to the `resampled_interval` and cropped according to the `max_arc_length`. +With the resulting preprocessed path points and previous curvatures, curvatures of the new path points are calculated using the 3 points method and smoothed using a moving window average with window size `curvature_average_window`. + +##### 2 For each path point, calculate the closest bound segment and the minimum drivable area width + +Each path point is projected on the original left and right drivable area bounds to calculate its corresponding bound index, original distance from the bounds, and the projected point. +Additionally, for each path point, the minimum drivable area width is calculated using the following equation: +$$ W = \frac{a² + 2 al + 2kw + l² + w²}{2k + w}$$ +Where $W$ is the minimum drivable area width, $a$, is the front overhang of ego, $l$ is the wheelbase of ego, $w$ is the width of ego, and $k$ is the path curvature. +This equation was derived from the work of [Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021](https://www.sae.org/publications/technical-papers/content/2021-01-0099/). + +![min width](../images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg) + +##### 3 Calculate maximum expansion distances of each bound point based on dynamic objects and linestring of the vector map (optional) + +For each drivable area bound point, we calculate its maximum expansion distance as its distance to the closest "obstacle" (either a map linestring with type `avoid_linestrings.type`, or a dynamic object footprint if `dynamic_objects.avoid` is set to `true`). +If `max_expansion_distance` is not `0.0`, it is use here if smaller than the distance to the closest obstacle. + +![max distances](../images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg) + +##### 4 Calculate by how much each bound point should be pushed away from the path + +For each bound point, a shift distance is calculated. +such that the resulting width between corresponding left and right bound points is as close as possible to the minimum width calculated in step 2 but the individual shift distance stays bellow the previously calculated maximum expansion distance. + +![expansion distances](../images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg) + +##### 5 Shift bound points by the values calculated in step 4 and remove all loops in the resulting bound + +Finally, each bound point is shifted away from the path by the distance calculated in step 4. +Once all points have been shifted, loops are removed from the bound and we obtain our final expanded drivable area. + +| | | +| ------------------------------------------------------------------------------ | ------------------------------------------------------------------------ | +| ![expansion](../images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg) | ![result](../images/drivable_area/DynamicDrivableArea-Result.drawio.svg) | + +### Visualizing maximum drivable area (Debug) + +Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. + +For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. + +To debug the issue, the maximum drivable area boundary can be visualized. + +![drivable_area_boundary_marker1](../images/drivable_area/drivable_area_boundary_marker_example1.png) + +![drivable_area_boundary_marker2](../images/drivable_area/drivable_area_boundary_marker_example2.png) + +The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` + +#### Expansion with hatched road markings area + +If the hatched road markings area is defined in the lanelet map, the area can be used as a drivable area. +Since the area is expressed as a polygon format of Lanelet2, several steps are required for correct expansion. + +![hatched_road_markings_drivable_area_expansion](../images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md similarity index 97% rename from planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md rename to planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md index 7784f96543ea6..2a22db5597241 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md @@ -1,6 +1,6 @@ # Path Generation design -This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp). +This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp). ## Overview @@ -8,7 +8,7 @@ The base idea of the path generation in lane change and avoidance is to smoothly The figure below explains how the application of a constant lateral jerk $l^{'''}(s)$ can be used to induce lateral shifting. In order to comply with the limits on lateral acceleration and velocity, zero-jerk time is employed in the figure ( $T_a$ and $T_v$ ). In each interval where constant jerk is applied, the shift position $l(s)$ can be characterized by a third-degree polynomial. Therefore the shift length from the reference path can then be calculated by combining spline curves. -![path-shifter](../image/path_shifter.png) +![path-shifter](../images/path_shifter/path_shifter.png) Note that, due to the rarity of the $T_v$ in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md similarity index 93% rename from planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md rename to planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md index 30e2093cb465e..4733fe7832da6 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md @@ -22,7 +22,7 @@ Currently the yaw angle of each point of predicted paths of a target object does The flow of the safety check algorithm is described in the following explanations. -![safety_check_flow](../image/path_safety_checker/safety_check_flow.drawio.svg) +![safety_check_flow](../images/path_safety_checker/safety_check_flow.drawio.svg) Here we explain each step of the algorithm flow. @@ -38,7 +38,7 @@ With the interpolated pose obtained in the step.1, we check if the object and eg After the overlap check, it starts to perform the safety check for the broader range. In this step, it judges if ego or target object is in front of the other vehicle. We use arc length of the front point of each object along the given path to judge which one is in front of the other. In the following example, target object (red rectangle) is running in front of the ego vehicle (black rectangle). -![front_object](../image/path_safety_checker/front_object.drawio.svg) +![front_object](../images/path_safety_checker/front_object.drawio.svg) #### 4. Calculate RSS distance @@ -54,7 +54,7 @@ where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively a In this step, we compute extended ego and target object polygons. The extended polygons can be described as: -![extended_polygons](../image/path_safety_checker/extended_polygons.drawio.svg) +![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg) As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md similarity index 86% rename from planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md rename to planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md index 8b64db997aec6..c183e0627a674 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md @@ -35,7 +35,7 @@ Note that the default values for `turn_signal_intersection_search_distance` and In this algorithm, it assumes that each blinker has two sections, which are `desired section` and `required section`. The image of these two sections are depicted in the following diagram. -![section_definition](../image/turn_signal_decider/sections_definition.drawio.svg) +![section_definition](../images/turn_signal_decider/sections_definition.drawio.svg) These two sections have the following meanings. @@ -51,7 +51,7 @@ These two sections have the following meanings. When turning on the blinker, it decides whether or not to turn on the specified blinker based on the distance from the front of the ego vehicle to the start point of each section. Conversely, when turning off the blinker, it calculates the distance from the base link of the ego vehicle to the end point of each section and decide whether or not to turn it off based on that. -![activation_distance](../image/turn_signal_decider/activation_distance.drawio.svg) +![activation_distance](../images/turn_signal_decider/activation_distance.drawio.svg) For left turn, right turn, avoidance, lane change, goal planner and pull out, we define these two sections, which are elaborated in the following part. @@ -71,7 +71,7 @@ Turn signal decider checks each lanelet on the map if it has `turn_direction` in - required end point The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user. -![section_turn_signal](../image/turn_signal_decider/left_right_turn.drawio.svg) +![section_turn_signal](../images/turn_signal_decider/left_right_turn.drawio.svg) #### 2. Avoidance @@ -91,7 +91,7 @@ First section - required end point Shift complete point same as the desired end point. -![section_first_avoidance](../image/turn_signal_decider/avoidance_first_section.drawio.svg) +![section_first_avoidance](../images/turn_signal_decider/avoidance_first_section.drawio.svg) Second section @@ -107,7 +107,7 @@ Second section - required end point Avoidance terminal point. -![section_second_avoidance](../image/turn_signal_decider/avoidance_second_section.drawio.svg) +![section_second_avoidance](../images/turn_signal_decider/avoidance_second_section.drawio.svg) #### 3. Lane Change @@ -123,7 +123,7 @@ Second section - required end point Cross point with lane change path and boundary line of the adjacent lane. -![section_lane_change](../image/turn_signal_decider/lane_change.drawio.svg) +![section_lane_change](../images/turn_signal_decider/lane_change.drawio.svg) #### 4. Pull out @@ -139,7 +139,7 @@ Second section - required end point Terminal point of the path of pull out. -![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg) +![section_pull_out](../images/turn_signal_decider/pull_out.drawio.svg) #### 5. Goal Planner @@ -155,24 +155,24 @@ Second section - required end point Terminal point of the path of pull over. -![section_pull_over](../image/turn_signal_decider/pull_over.drawio.svg) +![section_pull_over](../images/turn_signal_decider/pull_over.drawio.svg) When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal. - pattern1 - ![pattern1](../image/turn_signal_decider/pattern1.drawio.svg) + ![pattern1](../images/turn_signal_decider/pattern1.drawio.svg) - pattern2 - ![pattern2](../image/turn_signal_decider/pattern2.drawio.svg) + ![pattern2](../images/turn_signal_decider/pattern2.drawio.svg) - pattern3 - ![pattern3](../image/turn_signal_decider/pattern3.drawio.svg) + ![pattern3](../images/turn_signal_decider/pattern3.drawio.svg) - pattern4 - ![pattern4](../image/turn_signal_decider/pattern4.drawio.svg) + ![pattern4](../images/turn_signal_decider/pattern4.drawio.svg) - pattern5 - ![pattern5](../image/turn_signal_decider/pattern5.drawio.svg) + ![pattern5](../images/turn_signal_decider/pattern5.drawio.svg) Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts. @@ -180,22 +180,22 @@ Based on these five rules, turn signal decider can solve `blinker conflicts`. Th In this scenario, ego vehicle has to pass several turns that are close each other. Since this pattern can be solved by the pattern1 rule, the overall result is depicted in the following picture. -![continuous_turns](../image/turn_signal_decider/continuous_turns.drawio.svg) +![continuous_turns](../images/turn_signal_decider/continuous_turns.drawio.svg) #### - Avoidance with left turn (1) In this scene, ego vehicle has to deal with the obstacle that is on its original path as well as make a left turn. The overall result can be varied by the position of the obstacle, but the image of the result is described in the following picture. -![avoidance_and_turn](../image/turn_signal_decider/avoidance_and_turn.drawio.svg) +![avoidance_and_turn](../images/turn_signal_decider/avoidance_and_turn.drawio.svg) #### - Avoidance with left turn (2) Same as the previous scenario, ego vehicle has to avoid the obstacle as well as make a turn left. However, in this scene, the obstacle is parked after the intersection. Similar to the previous one, the overall result can be varied by the position of the obstacle, but the image of the result is described in the following picture. -![avoidance_and_turn](../image/turn_signal_decider/avoidance_and_turn2.drawio.svg) +![avoidance_and_turn](../images/turn_signal_decider/avoidance_and_turn2.drawio.svg) #### - Lane change and left turn In this scenario, ego vehicle has to do lane change before making a left turn. In the following example, ego vehicle does not activate left turn signal until it reaches the end point of the lane change path. -![avoidance_and_turn](../image/turn_signal_decider/lane_change_and_turn.drawio.svg) +![avoidance_and_turn](../images/turn_signal_decider/lane_change_and_turn.drawio.svg) diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg new file mode 100644 index 0000000000000..163e5fbba719a --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg @@ -0,0 +1,106 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + + + + + + + + + + +
+
+
+
+ Maximum expansion distance +
+
+
+
+
+ + Maximum expansion distance + + + + + +
+
+
+
+ Obstacle +
+
+
+
+
+ + Obstacle + + + + + + + + + +
+
+
Expansion distances
+
+
+ + Expansion distances + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg new file mode 100644 index 0000000000000..a2b11b151db8c --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg @@ -0,0 +1,95 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + +
+
+
Expansion distances
+
+
+ + Expansion distances + + + + + + + + + + + + + + + +
+
+
Bound points
+
+
+ + Bound points + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg new file mode 100644 index 0000000000000..09a9260475dff --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg @@ -0,0 +1,56 @@ + + + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + +
+
+
+ Drivable area bounds +
+
+
+ + Drivable area bounds + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg new file mode 100644 index 0000000000000..7e1a823008fc8 --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg @@ -0,0 +1,84 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + + + + + + + + + + +
+
+
+
+ Maximum expansion distance +
+
+
+
+
+ + Maximum expansion distance + + + + + +
+
+
+
+ Obstacle +
+
+
+
+
+ + Obstacle + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg new file mode 100644 index 0000000000000..e6bc7a0137458 --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg @@ -0,0 +1,64 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + + + + + + + + + + +
+
+
+
+ Minimum drivable area width +
+
+
+
+
+ + Minimum drivable area width + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg new file mode 100644 index 0000000000000..9194cfc584b16 --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg @@ -0,0 +1,59 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + +
+
+
Bound points
+
+
+ + Bound points + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png new file mode 100644 index 0000000000000..49b4b382c344b Binary files /dev/null and b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png differ diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png new file mode 100644 index 0000000000000..9a9dbbc7e8180 Binary files /dev/null and b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png differ diff --git a/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg diff --git a/planning/behavior_path_planner/image/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg diff --git a/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg b/planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg rename to planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg b/planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg rename to planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg diff --git a/planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg b/planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg rename to planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg diff --git a/planning/behavior_path_planner/image/path_shifter.png b/planning/behavior_path_planner_common/images/path_shifter/path_shifter.png similarity index 100% rename from planning/behavior_path_planner/image/path_shifter.png rename to planning/behavior_path_planner_common/images/path_shifter/path_shifter.png diff --git a/planning/behavior_path_planner/image/turn_signal_decider/activation_distance.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/activation_distance.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn2.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn2.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_first_section.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_first_section.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_second_section.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_second_section.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/continuous_turns.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/continuous_turns.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/lane_change.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/lane_change.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/lane_change_and_turn.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/lane_change_and_turn.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/left_right_turn.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/left_right_turn.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern1.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern1.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern2.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern2.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern3.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern3.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern4.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern4.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern5.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern5.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pull_out.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pull_out.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pull_over.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pull_over.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/sections_definition.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/sections_definition.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 3dcfdcc2bdcef..31dc117fbce35 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -21,7 +21,8 @@ #include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include +#include #include #include @@ -38,12 +39,11 @@ #include #include -#include - #include #include #include #include +#include #include namespace behavior_path_planner @@ -100,7 +100,7 @@ struct DrivableAreaInfo { geometry_msgs::msg::Pose pose; tier4_autoware_utils::Polygon2d poly; - bool is_left; + bool is_left{true}; }; std::vector drivable_lanes{}; std::vector obstacles{}; // obstacles to extract from the drivable area @@ -119,10 +119,10 @@ struct BehaviorModuleOutput BehaviorModuleOutput() = default; // path planed by module - PlanResult path{}; + PathWithLaneId path{}; // reference path planed by module - PlanResult reference_path{}; + PathWithLaneId reference_path{}; TurnSignalInfo turn_signal_info{}; @@ -136,7 +136,7 @@ struct BehaviorModuleOutput struct CandidateOutput { CandidateOutput() = default; - explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {} + explicit CandidateOutput(PathWithLaneId path) : path_candidate{std::move(path)} {} PathWithLaneId path_candidate{}; double lateral_shift{0.0}; double start_distance_to_path_change{std::numeric_limits::lowest()}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index a5e2c3245f3c3..1a8c8241abe1a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -111,21 +111,6 @@ class SceneModuleInterface virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; - /** - * @brief Set the current_state_ based on updateState output. - */ - virtual void updateCurrentState() - { - const auto print = [this](const auto & from, const auto & to) { - RCLCPP_DEBUG( - getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); - }; - - const auto & from = current_state_; - current_state_ = updateState(); - print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); - } - /** * @brief Return true if the module has request for execution (not necessarily feasible) */ @@ -159,6 +144,21 @@ class SceneModuleInterface return isWaitingApproval() ? planWaitingApproval() : plan(); } + /** + * @brief Set the current_state_ based on updateState output. + */ + void updateCurrentState() + { + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG( + getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); + }; + + const auto & from = current_state_; + current_state_ = updateState(); + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); + } + /** * @brief Called on the first time when the module goes into RUNNING. */ @@ -259,6 +259,8 @@ class SceneModuleInterface return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL; } + virtual bool isRootLaneletToBeUpdated() const { return false; } + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } PlanResult getPathCandidate() const { return path_candidate_; } @@ -360,9 +362,83 @@ class SceneModuleInterface return existApprovedRequest(); } + /** + * @brief Return SUCCESS if plan is not needed or plan is successfully finished, + * FAILURE if plan has failed, RUNNING if plan is on going. + * These condition is to be implemented in each modules. + */ + ModuleStatus updateState() + { + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + if (current_state_ == ModuleStatus::IDLE) { + if (canTransitIdleToRunningState()) { + log_debug_throttled("transiting from IDLE to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from IDLE to IDLE"); + return ModuleStatus::IDLE; + } + + if (current_state_ == ModuleStatus::RUNNING) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from RUNNING to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from RUNNING to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalState()) { + log_debug_throttled("transiting from RUNNING to WAITING_APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + log_debug_throttled("transiting from RUNNING to RUNNING"); + return ModuleStatus::RUNNING; + } + + if (current_state_ == ModuleStatus::WAITING_APPROVAL) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalToRunningState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from WAITING_APPROVAL to WAITING APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + if (current_state_ == ModuleStatus::SUCCESS) { + log_debug_throttled("already SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (current_state_ == ModuleStatus::FAILURE) { + log_debug_throttled("already FAILURE"); + return ModuleStatus::FAILURE; + } + + log_debug_throttled("already IDLE"); + return ModuleStatus::IDLE; + } + std::string name_; - rclcpp::Logger logger_; + ModuleStatus current_state_{ModuleStatus::IDLE}; BehaviorModuleOutput previous_module_output_; @@ -405,7 +481,7 @@ class SceneModuleInterface virtual BehaviorModuleOutput planWaitingApproval() { path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return getPreviousModuleOutput(); } @@ -442,64 +518,6 @@ class SceneModuleInterface } } - /** - * @brief Return SUCCESS if plan is not needed or plan is successfully finished, - * FAILURE if plan has failed, RUNNING if plan is on going. - * These condition is to be implemented in each modules. - */ - virtual ModuleStatus updateState() - { - if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToRunningState()) { - return ModuleStatus::RUNNING; - } - - return ModuleStatus::IDLE; - } - - if (current_state_ == ModuleStatus::RUNNING) { - if (canTransitSuccessState()) { - return ModuleStatus::SUCCESS; - } - - if (canTransitFailureState()) { - return ModuleStatus::FAILURE; - } - - if (canTransitWaitingApprovalState()) { - return ModuleStatus::WAITING_APPROVAL; - } - - return ModuleStatus::RUNNING; - } - - if (current_state_ == ModuleStatus::WAITING_APPROVAL) { - if (canTransitSuccessState()) { - return ModuleStatus::SUCCESS; - } - - if (canTransitFailureState()) { - return ModuleStatus::FAILURE; - } - - if (canTransitWaitingApprovalToRunningState()) { - return ModuleStatus::RUNNING; - } - - return ModuleStatus::WAITING_APPROVAL; - } - - if (current_state_ == ModuleStatus::SUCCESS) { - return ModuleStatus::SUCCESS; - } - - if (current_state_ == ModuleStatus::FAILURE) { - return ModuleStatus::FAILURE; - } - - return ModuleStatus::IDLE; - } - /** * @brief Return true if the activation command is received from the RTC interface. * If no RTC interface is registered, return true. @@ -581,6 +599,8 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; @@ -592,8 +612,6 @@ class SceneModuleInterface PlanResult path_candidate_; PlanResult path_reference_; - ModuleStatus current_state_{ModuleStatus::IDLE}; - std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 95976bf117096..43ecd17fa1844 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -19,10 +19,13 @@ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -#include +#include +#include +#include #include +#include #include #include #include @@ -44,7 +47,11 @@ using SceneModuleObserver = std::weak_ptr; class SceneModuleManagerInterface { public: - explicit SceneModuleManagerInterface(const std::string & name) : name_{name} {} + SceneModuleManagerInterface(const SceneModuleManagerInterface &) = delete; + SceneModuleManagerInterface(SceneModuleManagerInterface &&) = delete; + SceneModuleManagerInterface & operator=(const SceneModuleManagerInterface &) = delete; + SceneModuleManagerInterface & operator=(SceneModuleManagerInterface &&) = delete; + explicit SceneModuleManagerInterface(std::string name) : name_{std::move(name)} {} virtual ~SceneModuleManagerInterface() = default; @@ -275,7 +282,7 @@ class SceneModuleManagerInterface for (const auto & rtc_type : rtc_types) { const auto snake_case_name = utils::convertToSnakeCase(name_); const auto rtc_interface_name = - rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; + rtc_type.empty() ? snake_case_name : snake_case_name + "_" + rtc_type; rtc_interface_ptr_map_.emplace( rtc_type, std::make_shared(node, rtc_interface_name, config_.enable_rtc)); objects_of_interest_marker_interface_ptr_map_.emplace( @@ -298,7 +305,7 @@ class SceneModuleManagerInterface virtual std::unique_ptr createNewSceneModuleInstance() = 0; - rclcpp::Node * node_; + rclcpp::Node * node_ = nullptr; rclcpp::Publisher::SharedPtr pub_info_marker_; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp index fd3ce0097c2eb..fb0cb97a3db95 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp @@ -37,7 +37,7 @@ class SteeringFactorInterface void updateSteeringFactor( const std::array & poses, const std::array distances, const std::string & behavior, const uint16_t direction, const uint16_t status, - const std::string detail); + const std::string & detail); void clearSteeringFactors(); private: diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp index 95dcff8602398..1c066ff744f46 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp @@ -16,7 +16,7 @@ #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "std_msgs/msg/color_rgba.hpp" +#include "std_msgs/msg/detail/color_rgba__struct.hpp" #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index cfaa8de9b2fb9..6426f16a44259 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -18,6 +18,8 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include #include #include @@ -56,6 +58,18 @@ inline int64_t bitShift(int64_t original_id) return original_id << (sizeof(int32_t) * 8 / 2); } +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); @@ -81,8 +95,6 @@ MarkerArray createLaneletsAreaMarkerArray( const std::vector & lanelets, std::string && ns, const float & r, const float & g, const float & b); -MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings); - MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b, const float & w = 0.3); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 5bdd0a2f3f88d..811c89f61c066 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -17,9 +17,6 @@ #include -#include -#include - struct ModuleConfigParameters { bool enable_module{false}; @@ -79,9 +76,6 @@ struct BehaviorPathPlannerParameters double right_over_hang; double base_link2front; double base_link2rear; - - // maximum drivable area visualization - bool visualize_maximum_drivable_area; }; #endif // BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 372041c7fb586..cd639b5e81092 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -39,7 +39,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; -const std::map signal_map = { +const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, {"right", TurnIndicatorsCommand::ENABLE_RIGHT}, {"straight", TurnIndicatorsCommand::DISABLE}, diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp deleted file mode 100644 index edb6d32bda205..0000000000000 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ - -#include -#include - -inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) -{ - using tier4_autoware_utils::LinearRing2d; - using tier4_autoware_utils::Point2d; - - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m + margin; - const double x_center = i.wheel_base_m / 2.0; - const double x_rear = -(i.rear_overhang_m + margin); - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; - const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); - - LinearRing2d footprint; - footprint.push_back(Point2d{x_front, y_left}); - footprint.push_back(Point2d{x_front, y_right}); - footprint.push_back(Point2d{x_center, y_right}); - footprint.push_back(Point2d{x_rear, y_right}); - footprint.push_back(Point2d{x_rear, y_left}); - footprint.push_back(Point2d{x_center, y_left}); - footprint.push_back(Point2d{x_front, y_left}); - - return footprint; -} - -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index d0eec8f2b8901..7e4cf94c890d8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -35,14 +35,14 @@ void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data); -/// @brief prepare path poses and try to reuse their previously calculated curvatures +/// @brief try to reuse the previous path poses and their previously calculated curvatures /// @details poses are reused if they do not deviate too much from the current path /// @param [in] path input path /// @param [inout] prev_poses previous poses to reuse /// @param [inout] prev_curvatures previous curvatures to reuse /// @param [in] ego_point current ego point /// @param [in] params parameters for reuse criteria and resampling interval -void update_path_poses_and_previous_curvatures( +void reuse_previous_poses( const PathWithLaneId & path, std::vector & prev_poses, std::vector & prev_curvatures, const Point & ego_point, const DrivableAreaExpansionParameters & params); @@ -57,6 +57,36 @@ void update_path_poses_and_previous_curvatures( double calculate_minimum_lane_width( const double curvature_radius, const DrivableAreaExpansionParameters & params); +/// @brief calculate mappings between path poses and the given drivable area bound +/// @param [inout] expansion expansion data to update with the mapping +/// @param [in] path_poses path poses +/// @param [in] bound drivable area bound +/// @param [in] Side left or right side +void calculate_bound_index_mappings( + Expansion & expansion, const std::vector & path_poses, const std::vector & bound, + const Side side); + +/// @brief apply expansion distances to all bound points within the given arc length range +/// @param [inout] expansion expansion data to update +/// @param [in] bound drivable area bound +/// @param [in] arc_length_range [m] arc length range where the expansion distances are also applied +/// @param [in] Side left or right side +void apply_arc_length_range_smoothing( + Expansion & expansion, const std::vector & bound, const double arc_length_range, + const Side side); + +/// @brief calculate minimum lane widths and mappings between path and and drivable area bounds +/// @param [in] path_poses path poses +/// @param [in] left_bound left drivable area bound +/// @param [in] right_bound right drivable area bound +/// @param [in] curvatures curvatures at each path point +/// @param [in] params parameters with the vehicle dimensions used to calculate the min lane width +/// @return expansion data (path->bound mappings, min lane widths, ...) +Expansion calculate_expansion( + const std::vector & path_poses, const std::vector & left_bound, + const std::vector & right_bound, const std::vector & curvatures, + const DrivableAreaExpansionParameters & params); + /// @brief smooth the bound by applying a limit on its rate of change /// @details rate of change is the lateral distance from the path over the arc length along the path /// @param [inout] bound_distances bound distances (lateral distance from the path) @@ -66,16 +96,16 @@ void apply_bound_change_rate_limit( std::vector & distances, const std::vector & bound, const double max_rate); /// @brief calculate the maximum distance by which a bound can be expanded -/// @param [in] path_poses input path /// @param [in] bound bound points /// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree /// @param [in] uncrossable_polygons polygons that limit the bound expansion /// @param [in] params parameters with the buffer distance to keep with lines, /// and the static maximum expansion distance +/// @param [in] Side left or right side std::vector calculate_maximum_distance( - const std::vector & path_poses, const std::vector bound, - const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, - const DrivableAreaExpansionParameters & params); + const std::vector & bound, const SegmentRtree & uncrossable_lines, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params, const Side side); /// @brief expand a bound by the given lateral distances away from the path /// @param [inout] bound bound points to expand @@ -85,6 +115,14 @@ void expand_bound( std::vector & bound, const std::vector & path_poses, const std::vector & distances); +/// @brief calculate the expansion distances of the left and right drivable area bounds +/// @param [inout] expansion expansion data to be updated with the left/right expansion distances +/// @param [in] max_left_expansions maximum left expansion distances +/// @param [in] max_right_expansions maximum right expansion distances +void calculate_expansion_distances( + Expansion & expansion, const std::vector & max_left_expansions, + const std::vector & max_right_expansions); + /// @brief calculate smoothed curvatures /// @details smoothing is done using a moving average /// @param [in] poses input poses used to calculate the curvatures diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 297092d0cdd68..2a4343e2c3f6a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -22,13 +22,6 @@ #include #include -#include - -#include -#include -#include -#include -#include namespace drivable_area_expansion { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 6d1497397ad27..055e20a4cda02 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -55,8 +55,8 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.smoothing.curvature_average_window"; static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = "dynamic_expansion.smoothing.max_bound_rate"; - static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM = - "dynamic_expansion.smoothing.extra_arc_length"; + static constexpr auto SMOOTHING_ARC_LENGTH_RANGE_PARAM = + "dynamic_expansion.smoothing.arc_length_range"; static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; // static expansion @@ -78,7 +78,7 @@ struct DrivableAreaExpansionParameters double max_expansion_distance{}; double max_path_arc_length{}; double resample_interval{}; - double extra_arc_length{}; + double arc_length_range{}; double max_reuse_deviation{}; bool avoid_dynamic_objects{}; bool print_runtime{}; @@ -103,7 +103,7 @@ struct DrivableAreaExpansionParameters extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); - extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); + arc_length_range = node.declare_parameter(SMOOTHING_ARC_LENGTH_RANGE_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 4c1469d46e03a..2aeac123623ce 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -19,14 +19,13 @@ #include -#include +#include #include namespace drivable_area_expansion { /// @brief project a point to a segment -/// @details the distance is signed based on the side of the point: left=positive, right=negative /// @param p point to project on the segment /// @param p1 first segment point /// @param p2 second segment point @@ -37,22 +36,18 @@ inline PointDistance point_to_segment_projection( const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; - const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); - const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; - const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); - if (c1 <= 0) return {p1, boost::geometry::distance(p, p1) * dist_sign}; + if (c1 <= 0) return {p1, boost::geometry::distance(p, p1)}; const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); - if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; + if (c2 <= c1) return {p2, boost::geometry::distance(p, p2)}; const auto projection = p1 + (p2_vec * c1 / c2); const auto projection_point = Point2d{projection.x(), projection.y()}; - return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; + return {projection_point, boost::geometry::distance(p, projection_point)}; } /// @brief project a point to a line -/// @details the distance is signed based on the side of the point: left=positive, right=negative /// @param p point to project on the line /// @param p1 first line point /// @param p2 second line point @@ -63,14 +58,11 @@ inline PointDistance point_to_line_projection( const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; - const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); - const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; - const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); const auto projection = p1 + (p2_vec * c1 / c2); const auto projection_point = Point2d{projection.x(), projection.y()}; - return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; + return {projection_point, boost::geometry::distance(p, projection_point)}; } /// @brief project a point to a linestring @@ -84,7 +76,7 @@ inline Projection point_to_linestring_projection(const Point2d & p, const LineSt auto arc_length = 0.0; for (auto ls_it = ls.begin(); std::next(ls_it) != ls.end(); ++ls_it) { const auto pd = point_to_segment_projection(p, *ls_it, *(ls_it + 1)); - if (std::abs(pd.distance) < std::abs(closest.distance)) { + if (pd.distance < closest.distance) { closest.projected_point = pd.point; closest.distance = pd.distance; closest.arc_length = arc_length + boost::geometry::distance(*ls_it, pd.point); @@ -141,13 +133,11 @@ inline Segment2d linestring_to_point_projection( const auto lerp_ratio = (arc_length - prev_arc_length) / (curr_arc_length - prev_arc_length); const auto base_point = lerp_point(*std::prev(ls_iterator), *ls_iterator, lerp_ratio); - if (distance == 0.0) - return {base_point, base_point}; - else if (lerp_ratio >= 1.0) // base point is beyond the 2nd segment point -> calculate normal in - // the other direction + if (distance == 0.0) return {base_point, base_point}; + if (lerp_ratio >= 1.0) // base point is beyond the 2nd segment point -> calculate normal in + // the other direction return {base_point, normal_at_distance(base_point, *std::prev(ls_iterator), -distance)}; - else - return {base_point, normal_at_distance(base_point, *ls_iterator, distance)}; + return {base_point, normal_at_distance(base_point, *ls_iterator, distance)}; } /// @brief create a sub linestring between the given arc lengths diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 284b4aad20a6a..9053da45708a0 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -47,9 +47,6 @@ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward = true); -lanelet::ConstLineStrings3d getMaximumDrivableArea( - const std::shared_ptr & planner_data); - /** * @brief Expand the borders of the given lanelets * @param [in] drivable_lanes lanelets to expand diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index eae53b027655c..e438105c6645e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -24,6 +24,9 @@ #include +#include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -41,20 +44,31 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::Segment2d; -typedef boost::geometry::index::rtree> SegmentRtree; +using SegmentRtree = boost::geometry::index::rtree>; struct PointDistance { - Point2d point; - double distance; + Point2d point{}; + double distance{}; }; struct Projection { - Point2d projected_point; - double distance; - double arc_length; + Point2d projected_point{}; + double distance{}; + double arc_length{}; }; enum Side { LEFT, RIGHT }; - +struct Expansion +{ + // mappings from bound index + std::vector left_distances; + std::vector right_distances; + // mappings from path index + std::vector left_bound_indexes; + std::vector left_projections; + std::vector right_bound_indexes; + std::vector right_projections; + std::vector min_lane_widths; +}; } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp similarity index 79% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 91d99fcd4a0d6..5b1e9b4b4c419 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #include @@ -21,14 +21,6 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include namespace behavior_path_planner @@ -74,8 +66,8 @@ struct OccupancyGridMapParam VehicleShape vehicle_shape; // costmap configs - int theta_size; // discretized angle table size [-] - int obstacle_threshold; // obstacle threshold on grid [-] + int theta_size{0}; // discretized angle table size [-] + int obstacle_threshold{0}; // obstacle threshold on grid [-] }; struct PlannerWaypoint @@ -93,7 +85,12 @@ struct PlannerWaypoints class OccupancyGridBasedCollisionDetector { public: - OccupancyGridBasedCollisionDetector() {} + OccupancyGridBasedCollisionDetector() = default; + OccupancyGridBasedCollisionDetector(const OccupancyGridBasedCollisionDetector &) = default; + OccupancyGridBasedCollisionDetector(OccupancyGridBasedCollisionDetector &&) = delete; + OccupancyGridBasedCollisionDetector & operator=(const OccupancyGridBasedCollisionDetector &) = + default; + OccupancyGridBasedCollisionDetector & operator=(OccupancyGridBasedCollisionDetector &&) = delete; void setParam(const OccupancyGridMapParam & param) { param_ = param; }; OccupancyGridMapParam getParam() const { return param_; }; void setMap(const nav_msgs::msg::OccupancyGrid & costmap); @@ -106,7 +103,7 @@ class OccupancyGridBasedCollisionDetector const bool check_out_of_range) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; - virtual ~OccupancyGridBasedCollisionDetector() {} + virtual ~OccupancyGridBasedCollisionDetector() = default; protected: void computeCollisionIndexes(int theta_index, std::vector & indexes); @@ -149,4 +146,4 @@ class OccupancyGridBasedCollisionDetector } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index c270a1500431c..3dc230f0e92ed 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -17,8 +17,6 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include @@ -40,6 +38,11 @@ struct StartGoalPlannerData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; + + Pose refined_start_pose; + std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 4d1f1fdb959c0..26c0f247eb558 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -18,15 +18,10 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" -#include -#include - #include #include #include -#include -#include -#include +#include #include @@ -40,7 +35,6 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp index 1d4245bfec0e4..e1f3402e8dfdf 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -19,8 +19,6 @@ #include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include #include @@ -72,7 +70,7 @@ std::pair getPairsTerminalVelocityAndAccel( std::optional generateFeasibleStopPath( PathWithLaneId & current_path, std::shared_ptr planner_data, - geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + std::optional & stop_pose, const double maximum_deceleration, const double maximum_jerk); /** diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f66f425f1b7fa..e33c2596ab04d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -23,8 +23,6 @@ #include #include -#include -#include #include #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index fcb4f3f4dc207..13009d5114439 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -26,14 +26,6 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 099adc21d2ad7..165a11ebb54b4 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -15,11 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#include +#include +#include #include #include +#include #include #include #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index 4ac6f01da65fb..f109bb2cbb213 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline( const PathWithLaneId & path, const double interval, const bool keep_input_points = false, const std::pair target_section = {0.0, std::numeric_limits::max()}); -Path toPath(const PathWithLaneId & input); - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index bf48252a01dcd..62fd87bbd91bc 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -37,43 +37,6 @@ using autoware_perception_msgs::msg::TrafficSignal; using autoware_perception_msgs::msg::TrafficSignalElement; using geometry_msgs::msg::Pose; -/** - * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. - * - * Iterates through the traffic light elements to find a circle-shaped light that matches the given - * color. - * - * @param tl_state The traffic light state to check. - * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. - * @return True if a circle-shaped light with the specified color is found, false otherwise. - */ -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color); - -/** - * @brief Checks if a traffic light state includes a light with the specified shape. - * - * Searches through the traffic light elements to find a light that matches the given shape. - * - * @param tl_state The traffic light state to check. - * @param shape The shape to look for in the traffic light's lights. - * @return True if a light with the specified shape is found, false otherwise. - */ -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape); - -/** - * @brief Determines if a traffic signal indicates a stop for the given lanelet. - * - * Evaluates the current state of the traffic light, considering if it's green or unknown, - * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet - * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can - * proceed based on allowed turn directions. - * - * @param lanelet The lanelet to check for a stop signal at its traffic light. - * @param tl_state The current state of the traffic light associated with the lanelet. - * @return True if the traffic signal indicates a stop is required, false otherwise. - */ -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state); - /** * @brief Computes the distance from the current position to the next traffic light along a set of * lanelets. @@ -129,6 +92,21 @@ bool isStoppedAtRedTrafficLightWithinDistance( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const double distance_threshold = std::numeric_limits::infinity()); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::traffic_light #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index e2c9fd1e332a2..01b2880f4362a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -34,18 +33,10 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif #include #include #include -#include #include namespace behavior_path_planner::utils @@ -55,34 +46,21 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; -using path_safety_checker::ExtendedPredictedObject; -using path_safety_checker::ObjectTypesToCheck; -using path_safety_checker::PoseWithVelocityAndPolygonStamped; -using path_safety_checker::PoseWithVelocityStamped; -using path_safety_checker::PredictedPathWithPolygon; -using path_safety_checker::SafetyCheckParams; -using path_safety_checker::TargetObjectsOnLane; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; struct PolygonPoint { geometry_msgs::msg::Point point; - size_t bound_seg_idx; - double lon_dist_to_segment; - double lat_dist_to_bound; + size_t bound_seg_idx{0}; + double lon_dist_to_segment{0.0}; + double lat_dist_to_bound{0.0}; bool is_after(const PolygonPoint & other_point) const { diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index b289ce86ca60e..db5dc51150e55 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -11,6 +11,8 @@ Satoshi Ota + Mamoru Sobue + Daniel Sanchez Maxime CLEMENT Takayuki Murooka Satoshi Ota @@ -48,8 +50,6 @@ geometry_msgs interpolation lanelet2_extension - libboost-dev - libopencv-dev magic_enum motion_utils object_recognition_utils @@ -57,13 +57,10 @@ rclcpp route_handler rtc_interface - sensor_msgs tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp b/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp index 93ae7e8289902..cbced86865a99 100644 --- a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp +++ b/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp @@ -34,12 +34,12 @@ void SteeringFactorInterface::publishSteeringFactor(const rclcpp::Time & stamp) void SteeringFactorInterface::updateSteeringFactor( const std::array & pose, const std::array distance, const std::string & behavior, const uint16_t direction, const uint16_t status, - const std::string detail) + const std::string & detail) { std::lock_guard lock(mutex_); SteeringFactor factor; factor.pose = pose; - std::array converted_distance; + std::array converted_distance{0.0, 0.0}; for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); factor.distance = converted_distance; factor.behavior = behavior; diff --git a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index 4dc422a81d22c..d97ea2e09a89d 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -30,9 +30,7 @@ namespace marker_utils { -using behavior_path_planner::ShiftLine; using behavior_path_planner::utils::calcPathArcLengthArray; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -42,6 +40,61 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) @@ -101,7 +154,7 @@ MarkerArray createShiftLineMarkerArray( { ShiftLineArray shift_lines_local = shift_lines; if (shift_lines.empty()) { - shift_lines_local.push_back(ShiftLine()); + shift_lines_local.emplace_back(); } MarkerArray msg; @@ -238,90 +291,6 @@ MarkerArray createLaneletsAreaMarkerArray( return msg; } -MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings) -{ - if (linestrings.empty()) { - return MarkerArray(); - } - - Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shared_linestring_lanelets", 0L, Marker::LINE_STRIP, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.996, 0.658, 0.466, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - - const auto reserve_size = linestrings.size() / 2; - lanelet::ConstLineStrings3d lefts; - lanelet::ConstLineStrings3d rights; - lefts.reserve(reserve_size); - rights.reserve(reserve_size); - - size_t total_marker_reserve_size{0}; - for (size_t idx = 1; idx < linestrings.size(); idx += 2) { - rights.emplace_back(linestrings.at(idx - 1)); - lefts.emplace_back(linestrings.at(idx)); - - for (const auto & ls : linestrings.at(idx - 1).basicLineString()) { - total_marker_reserve_size += ls.size(); - } - for (const auto & ls : linestrings.at(idx).basicLineString()) { - total_marker_reserve_size += ls.size(); - } - } - - if (!total_marker_reserve_size) { - marker.points.reserve(total_marker_reserve_size); - } - - const auto & first_ls = lefts.front().basicLineString(); - for (const auto & ls : first_ls) { - marker.points.push_back(createPoint(ls.x(), ls.y(), ls.z())); - } - - for (auto idx = lefts.cbegin() + 1; idx != lefts.cend(); ++idx) { - Point front = createPoint( - idx->basicLineString().front().x(), idx->basicLineString().front().y(), - idx->basicLineString().front().z()); - Point front_inverted = createPoint( - idx->invert().basicLineString().front().x(), idx->invert().basicLineString().front().y(), - idx->invert().basicLineString().front().z()); - - const auto & marker_back = marker.points.back(); - const bool isFrontNear = tier4_autoware_utils::calcDistance2d(marker_back, front) < - tier4_autoware_utils::calcDistance2d(marker_back, front_inverted); - const auto & left_ls = (isFrontNear) ? idx->basicLineString() : idx->invert().basicLineString(); - for (auto ls = left_ls.cbegin(); ls != left_ls.cend(); ++ls) { - marker.points.push_back(createPoint(ls->x(), ls->y(), ls->z())); - } - } - - for (auto idx = rights.crbegin(); idx != rights.crend(); ++idx) { - Point front = createPoint( - idx->basicLineString().front().x(), idx->basicLineString().front().y(), - idx->basicLineString().front().z()); - Point front_inverted = createPoint( - idx->invert().basicLineString().front().x(), idx->invert().basicLineString().front().y(), - idx->invert().basicLineString().front().z()); - - const auto & marker_back = marker.points.back(); - const bool isFrontFurther = tier4_autoware_utils::calcDistance2d(marker_back, front) > - tier4_autoware_utils::calcDistance2d(marker_back, front_inverted); - const auto & right_ls = - (isFrontFurther) ? idx->basicLineString() : idx->invert().basicLineString(); - for (auto ls = right_ls.crbegin(); ls != right_ls.crend(); ++ls) { - marker.points.push_back(createPoint(ls->x(), ls->y(), ls->z())); - } - } - - if (!marker.points.empty()) { - marker.points.push_back(marker.points.front()); - } - - MarkerArray msg; - - msg.markers.push_back(marker); - return msg; -} - MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b, const float & w) @@ -433,7 +402,6 @@ MarkerArray createPredictedPathMarkerArray( Marker marker = createDefaultMarker( "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), - createMarkerColor(r, g, b, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); @@ -443,34 +411,11 @@ MarkerArray createPredictedPathMarkerArray( marker.points.clear(); const auto & predicted_path_pose = path.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; - const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; - const double base_to_rear = vehicle_info.rear_overhang_m; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) - .position); - marker.points.push_back(marker.points.front()); + addFootprintMarker(marker, predicted_path_pose, vehicle_info); marker_array.markers.push_back(marker); } return marker_array; - - marker.points.reserve(path.size()); - for (const auto & point : path) { - marker.points.push_back(point.position); - } - msg.markers.push_back(marker); - return msg; } MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 933330dafae7e..44358dfa84e4e 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -266,7 +266,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( turn_signal_info.required_start_point = lane_front_pose; turn_signal_info.required_end_point = get_required_end_point(combined_lane.centerline3d()); turn_signal_info.desired_end_point = lane_back_pose; - turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); + turn_signal_info.turn_signal.command = g_signal_map.at(lane_attribute); signal_queue.push(turn_signal_info); } } diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 4fe9a3cd6e10f..da7f0fbb327d7 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,14 +20,13 @@ #include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include #include #include #include #include #include -#include +#include #include @@ -121,52 +120,78 @@ double calculate_minimum_lane_width( return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -std::vector calculate_minimum_expansions( - const std::vector & path_poses, const std::vector bound, - const std::vector curvatures, const Side side, - const DrivableAreaExpansionParameters & params) +void calculate_bound_index_mappings( + Expansion & expansion, const std::vector & path_poses, const std::vector & bound, + const Side side) { - std::vector minimum_expansions(bound.size()); size_t lb_idx = 0; + auto & bound_indexes = + (side == LEFT ? expansion.left_bound_indexes : expansion.right_bound_indexes); + auto & bound_projections = + (side == LEFT ? expansion.left_projections : expansion.right_projections); + bound_indexes.resize(path_poses.size(), 0LU); + bound_projections.resize(path_poses.size(), {{}, std::numeric_limits::max()}); for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { - const auto & path_pose = path_poses[path_idx]; - if (curvatures[path_idx] == 0.0) continue; - const auto curvature_radius = 1 / curvatures[path_idx]; - const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); - const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); - const auto offset_point = - tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; + const auto path_p = convert_point(path_poses[path_idx].position); for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { - const auto & prev_p = bound[bound_idx]; - const auto & next_p = bound[bound_idx + 1]; - const auto intersection_point = - tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); - if (intersection_point) { - lb_idx = bound_idx; - const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); - minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); - minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); - // apply the expansion to all bound points within the extra arc length - auto arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); - for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { - arc_length += - tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]); - if (arc_length > params.extra_arc_length) break; - minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); - } - arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); - for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { - const auto idx = bound_idx - down_offset_idx; - arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); - if (arc_length > params.extra_arc_length) break; - minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); - } - break; + const auto prev_p = convert_point(bound[bound_idx]); + const auto next_p = convert_point(bound[bound_idx + 1]); + const auto projection = point_to_segment_projection(path_p, prev_p, next_p); + if (projection.distance < bound_projections[path_idx].distance) { + bound_indexes[path_idx] = bound_idx; + bound_projections[path_idx] = projection; } } + lb_idx = bound_indexes[path_idx]; + } +} + +void apply_arc_length_range_smoothing( + Expansion & expansion, const std::vector & bound, const double arc_length_range, + const Side side) +{ + const auto & bound_indexes = + side == LEFT ? expansion.left_bound_indexes : expansion.right_bound_indexes; + const auto & bound_projections = + side == LEFT ? expansion.left_projections : expansion.right_projections; + auto & bound_expansions = side == LEFT ? expansion.left_distances : expansion.right_distances; + const auto original_expansions = bound_expansions; + for (auto path_idx = 0UL; path_idx < bound_indexes.size(); ++path_idx) { + const auto bound_idx = bound_indexes[path_idx]; + auto arc_length = boost::geometry::distance( + bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); + const auto update_arc_length_and_bound_expansions = [&](auto idx) { + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); + }; + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + update_arc_length_and_bound_expansions(up_bound_idx); + if (arc_length > arc_length_range) break; + } + arc_length = + boost::geometry::distance(bound_projections[path_idx].point, convert_point(bound[bound_idx])); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + update_arc_length_and_bound_expansions(bound_idx - down_offset_idx); + if (arc_length > arc_length_range) break; + } } - return minimum_expansions; +} + +Expansion calculate_expansion( + const std::vector & path_poses, const std::vector & left_bound, + const std::vector & right_bound, const std::vector & curvatures, + const DrivableAreaExpansionParameters & params) +{ + Expansion expansion; + expansion.min_lane_widths.resize(path_poses.size(), 0.0); + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + expansion.min_lane_widths[path_idx] = calculate_minimum_lane_width(curvature_radius, params); + } + calculate_bound_index_mappings(expansion, path_poses, left_bound, LEFT); + calculate_bound_index_mappings(expansion, path_poses, right_bound, RIGHT); + return expansion; } void apply_bound_change_rate_limit( @@ -185,21 +210,30 @@ void apply_bound_change_rate_limit( } std::vector calculate_maximum_distance( - const std::vector & path_poses, const std::vector bound, - const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, - const DrivableAreaExpansionParameters & params) + const std::vector & bound, const SegmentRtree & uncrossable_segments, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params, const Side side) { - // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) std::vector maximum_distances(bound.size(), std::numeric_limits::max()); - LineString2d path_ls; LineString2d bound_ls; for (const auto & p : bound) bound_ls.push_back(convert_point(p)); - for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + const auto segment_vector = segment_ls.second - segment_ls.first; + const auto is_point_on_correct_side = [&](const Point2d & p) { + const auto point_vector = p - segment_ls.first; + const auto cross_product = + (segment_vector.x() * point_vector.y() - segment_vector.y() * point_vector.x()); + return cross_product * (side == LEFT ? -1.0 : 1.0) <= 0.0; + }; + const auto is_on_correct_side = [&](const Segment2d & segment) { + return is_point_on_correct_side(segment.first) || is_point_on_correct_side(segment.second); + }; std::vector query_result; boost::geometry::index::query( - uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + uncrossable_segments, + boost::geometry::index::nearest(segment_ls, 1) && + boost::geometry::index::satisfies(is_on_correct_side), std::back_inserter(query_result)); if (!query_result.empty()) { const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); @@ -208,9 +242,18 @@ std::vector calculate_maximum_distance( maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); } for (const auto & uncrossable_poly : uncrossable_polygons) { - const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); - maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); - maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + if (boost::geometry::intersects(uncrossable_poly.outer(), segment_ls)) { + maximum_distances[i] = 0.0; + maximum_distances[i + 1] = 0.0; + break; + } + if (std::all_of( + uncrossable_poly.outer().begin(), uncrossable_poly.outer().end(), + is_point_on_correct_side)) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + } } } if (params.max_expansion_distance > 0.0) @@ -228,8 +271,7 @@ void expand_bound( if (expansions[idx] > 0.0) { const auto bound_p = convert_point(bound[idx]); const auto projection = point_to_linestring_projection(bound_p, path_ls); - const auto expansion_ratio = - (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + const auto expansion_ratio = (expansions[idx] + projection.distance) / projection.distance; const auto & path_p = projection.projected_point; const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); bound[idx].x = expanded_p.x(); @@ -271,6 +313,49 @@ std::vector calculate_smoothed_curvatures( } return smoothed_curvatures; } +void calculate_expansion_distances( + Expansion & expansion, const std::vector & max_left_expansions, + const std::vector & max_right_expansions) +{ + expansion.left_distances.resize(max_left_expansions.size(), 0.0); + expansion.right_distances.resize(max_right_expansions.size(), 0.0); + for (auto path_idx = 0UL; path_idx < expansion.min_lane_widths.size(); ++path_idx) { + const auto left_bound_idx = expansion.left_bound_indexes[path_idx]; + const auto right_bound_idx = expansion.right_bound_indexes[path_idx]; + const auto original_width = expansion.left_projections[path_idx].distance + + expansion.right_projections[path_idx].distance; + const auto expansion_dist = expansion.min_lane_widths[path_idx] - original_width; + if (expansion_dist <= 0.0) continue; + const auto left_limit = + std::min(max_left_expansions[left_bound_idx], max_left_expansions[left_bound_idx + 1]); + const auto right_limit = + std::min(max_right_expansions[right_bound_idx], max_right_expansions[right_bound_idx + 1]); + const auto expansion_fits_on_left_side = expansion_dist / 2.0 < left_limit; + const auto expansion_fits_on_right_side = expansion_dist / 2.0 < right_limit; + if (expansion_fits_on_left_side && expansion_fits_on_right_side) { + expansion.left_distances[left_bound_idx] = expansion_dist / 2.0; + expansion.left_distances[left_bound_idx + 1] = expansion_dist / 2.0; + expansion.right_distances[right_bound_idx] = expansion_dist / 2.0; + expansion.right_distances[right_bound_idx + 1] = expansion_dist / 2.0; + } else if (!expansion_fits_on_left_side) { + expansion.left_distances[left_bound_idx] = left_limit; + expansion.left_distances[left_bound_idx + 1] = left_limit; + const auto compensation_dist = expansion_dist / 2.0 - left_limit; + expansion.right_distances[right_bound_idx] = + std::min(right_limit, expansion_dist / 2.0 + compensation_dist); + expansion.right_distances[right_bound_idx + 1] = + std::min(right_limit, expansion_dist / 2.0 + compensation_dist); + } else if (!expansion_fits_on_right_side) { + expansion.right_distances[right_bound_idx] = right_limit; + expansion.right_distances[right_bound_idx + 1] = right_limit; + const auto compensation_dist = expansion_dist / 2.0 - right_limit; + expansion.left_distances[left_bound_idx] = + std::min(left_limit, expansion_dist / 2.0 + compensation_dist); + expansion.left_distances[left_bound_idx + 1] = + std::min(left_limit, expansion_dist / 2.0 + compensation_dist); + } + } +} void expand_drivable_area( PathWithLaneId & path, @@ -281,7 +366,6 @@ void expand_drivable_area( tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); - // crop first/last non deviating path_poses const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; const auto uncrossable_segments = extract_uncrossable_segments( @@ -304,32 +388,34 @@ void expand_drivable_area( const auto first_new_point_idx = curvatures.size(); curvatures.insert( curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); - auto left_expansions = - calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); - auto right_expansions = - calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); + auto expansion = + calculate_expansion(path_poses, path.left_bound, path.right_bound, curvatures, params); const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( - path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); + path.left_bound, uncrossable_segments, uncrossable_polygons, params, LEFT); const auto max_right_expansions = calculate_maximum_distance( - path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params); - for (auto i = 0LU; i < left_expansions.size(); ++i) - left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); - for (auto i = 0LU; i < right_expansions.size(); ++i) - right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); + path.right_bound, uncrossable_segments, uncrossable_polygons, params, RIGHT); const auto max_dist_ms = stop_watch.toc("max_dist"); + calculate_expansion_distances(expansion, max_left_expansions, max_right_expansions); + apply_arc_length_range_smoothing(expansion, path.left_bound, params.arc_length_range, LEFT); + apply_arc_length_range_smoothing(expansion, path.right_bound, params.arc_length_range, RIGHT); + stop_watch.tic("smooth"); - apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); - apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); + apply_bound_change_rate_limit(expansion.left_distances, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(expansion.right_distances, path.right_bound, params.max_bound_rate); const auto smooth_ms = stop_watch.toc("smooth"); + // reapply expansion limits that may have been broken by the previous smoothing + for (auto i = 0LU; i < expansion.left_distances.size(); ++i) + expansion.left_distances[i] = std::min(expansion.left_distances[i], max_left_expansions[i]); + for (auto i = 0LU; i < expansion.right_distances.size(); ++i) + expansion.right_distances[i] = std::min(expansion.right_distances[i], max_right_expansions[i]); - // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); - expand_bound(path.left_bound, path_poses, left_expansions); - expand_bound(path.right_bound, path_poses, right_expansions); + expand_bound(path.left_bound, path_poses, expansion.left_distances); + expand_bound(path.right_bound, path_poses, expansion.right_distances); const auto expand_ms = stop_watch.toc("expand"); const auto total_ms = stop_watch.toc("overall"); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 6b323be6c6328..787af681bf69b 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -19,10 +19,7 @@ #include #include -#include - -#include -#include +#include #include diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp index 2eb55599b83f9..8420ee96b0ba6 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -14,7 +14,7 @@ #include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index f6344b94a6bc5..44c3025a8c5d5 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -35,19 +35,22 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx), target_point); + return calcDistance2d(points.at(seg_idx), target_point); } if (segment_length < lon_dist) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx + 1), target_point); + return calcDistance2d(points.at(seg_idx + 1), target_point); } return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); }(); @@ -64,6 +67,49 @@ size_t findNearestSegmentIndexFromLateralDistance( return motion_utils::findNearestSegmentIndex(points, target_point); } +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Pose & target_point, + const double yaw_threshold) +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const auto base_yaw = tf2::getYaw(target_point.orientation); + const auto yaw = + normalizeRadian(calcAzimuthAngle(points.at(seg_idx), points.at(seg_idx + 1)) - base_yaw); + if (yaw_threshold < std::abs(yaw)) { + continue; + } + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point.position); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return calcDistance2d(points.at(seg_idx), target_point.position); + } + if (segment_length < lon_dist) { + return calcDistance2d(points.at(seg_idx + 1), target_point.position); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); + }(); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; + } + } + + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point.position); +} + bool checkHasSameLane( const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelet & target_lane) { @@ -102,6 +148,40 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } + +std::vector extractBoundFromPolygon( + const lanelet::ConstPolygon3d & polygon, const size_t start_idx, const size_t end_idx, + const bool clockwise) +{ + std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(polygon[i]); + + if (i + 1 == polygon.size() && clockwise) { + if (end_idx == 0) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = 0; + ret.push_back(polygon[i]); + continue; + } + + if (i == 0 && !clockwise) { + if (end_idx == polygon.size() - 1) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = polygon.size() - 1; + ret.push_back(polygon[i]); + continue; + } + } + + ret.push_back(polygon[end_idx]); + + return ret; +} } // namespace namespace behavior_path_planner::utils::drivable_area_processing @@ -586,48 +666,44 @@ std::vector cutOverlappedLanes( [&has_same_id_lane, &p](const auto & lanelet) { return has_same_id_lane(lanelet, p); }); }; - // Step1. find first path point within drivable lanes - size_t start_point_idx = std::numeric_limits::max(); - for (const auto & lanes : shorten_lanes) { - for (size_t i = 0; i < original_points.size(); ++i) { - // check right lane - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + const auto is_point_in_drivable_lanes = [&has_same_id_lane, &has_same_id_lanes]( + const auto & lanes, const auto & p) { + if (has_same_id_lane(lanes.right_lane, p)) { + return true; + } + // check left lane + if (has_same_id_lane(lanes.left_lane, p)) { + return true; + } + // check middle lanes + if (has_same_id_lanes(lanes.middle_lanes, p)) { + return true; + } + return false; + }; - // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + // Step1. find first path point within drivable lanes + size_t start_point_idx = original_points.size(); - // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + for (size_t i = 0; i < original_points.size(); ++i) { + const bool first_path_point_in_drivable_lane_found = std::any_of( + shorten_lanes.begin(), shorten_lanes.end(), + [&is_point_in_drivable_lanes, &original_points, i](const auto & lanes) { + return is_point_in_drivable_lanes(lanes, original_points.at(i)); + }); + if (first_path_point_in_drivable_lane_found) { + start_point_idx = i; + break; } } // Step2. pick up only path points within drivable lanes for (const auto & lanes : shorten_lanes) { for (size_t i = start_point_idx; i < original_points.size(); ++i) { - // check right lane - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + if (is_point_in_drivable_lanes(lanes, original_points.at(i))) { path.points.push_back(original_points.at(i)); continue; } - - // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { - path.points.push_back(original_points.at(i)); - continue; - } - - // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { - path.points.push_back(original_points.at(i)); - continue; - } - start_point_idx = i; break; } @@ -799,9 +875,9 @@ void generateDrivableArea( const auto front_pose = cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); const auto left_start_point = calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); const auto right_start_point = calcLongitudinalOffsetStartPoint( @@ -821,9 +897,9 @@ void generateDrivableArea( // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); const auto left_goal_point = calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); const auto right_goal_point = @@ -1012,34 +1088,6 @@ void generateDrivableArea( path.right_bound = modified_right_bound; } -// TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if -// we can refactor some of the code for better readability -lanelet::ConstLineStrings3d getMaximumDrivableArea( - const std::shared_ptr & planner_data) -{ - const auto & p = planner_data->parameters; - const auto & route_handler = planner_data->route_handler; - const auto & ego_pose = planner_data->self_odometry->pose.pose; - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "failed to find closest lanelet within route!!!"); - return {}; - } - - const auto current_lanes = route_handler->getLaneletSequence( - current_lane, ego_pose, p.backward_path_length, p.forward_path_length); - lanelet::ConstLineStrings3d linestring_shared; - for (const auto & lane : current_lanes) { - lanelet::ConstLineStrings3d furthest_line = route_handler->getFurthestLinestring(lane); - linestring_shared.insert(linestring_shared.end(), furthest_line.begin(), furthest_line.end()); - } - - return linestring_shared; -} - std::vector expandLanelets( const std::vector & drivable_lanes, const double left_bound_offset, const double right_bound_offset, const std::vector & types_to_skip) @@ -1089,13 +1137,17 @@ void extractObstaclesFromDrivableArea( std::vector> right_polygons; std::vector> left_polygons; for (const auto & obstacle : obstacles) { + if (obstacle.poly.outer().empty()) { + continue; + } + const auto & obj_pos = obstacle.pose.position; // get edge points of the object const size_t nearest_path_idx = motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon std::vector edge_points; - for (size_t i = 0; i < obstacle.poly.outer().size() - 1; + for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points edge_points.push_back(tier4_autoware_utils::createPoint( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), @@ -1183,6 +1235,8 @@ std::vector getBoundWithHatchedRoadMarkings( } else { if (!polygon) { will_close_polygon = true; + } else if (polygon.value().id() != current_polygon.value().id()) { + will_close_polygon = true; } else { current_polygon_border_indices.push_back( get_corresponding_polygon_index(*current_polygon, bound_point.id())); @@ -1217,6 +1271,17 @@ std::vector getBoundWithHatchedRoadMarkings( (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); } } + + if (polygon.has_value() && current_polygon.has_value()) { + if (polygon.value().id() != current_polygon.value().id()) { + current_polygon = polygon; + current_polygon_border_indices.clear(); + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(current_polygon.value(), bound_point.id())); + continue; + } + } + current_polygon = std::nullopt; current_polygon_border_indices.clear(); } @@ -1230,36 +1295,6 @@ std::vector getBoundWithIntersectionAreas( 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{}; - for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(polygon[i]); - - if (i + 1 == polygon.size() && clockwise) { - if (end_idx == 0) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = 0; - continue; - } - - if (i == 0 && !clockwise) { - if (end_idx == polygon.size() - 1) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = polygon.size() - 1; - continue; - } - } - - ret.push_back(polygon[end_idx]); - - return ret; - }; - std::vector expanded_bound = original_bound; // expand drivable area by using intersection area. @@ -1305,7 +1340,7 @@ std::vector getBoundWithIntersectionAreas( const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); intersection_bound = - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); + extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); } // Step2. check shared bound point. diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp rename to planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 0f87c68e38289..1aab58ac2ff6e 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 7ba4f114328a7..a1a079e679e72 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -23,38 +23,22 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include +#include #include -#endif - -#include +#include #include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::Transform; -using geometry_msgs::msg::TransformStamped; using lanelet::utils::getArcCoordinates; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::deg2rad; using tier4_autoware_utils::inverseTransformPoint; -using tier4_autoware_utils::inverseTransformPose; using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::toMsg; using tier4_autoware_utils::transformPose; namespace behavior_path_planner @@ -518,15 +502,11 @@ std::vector GeometricParallelParking::planOneTrial( // set terminal velocity and acceleration(temporary implementation) if (is_forward) { - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.forward_parking_velocity, 0.0)); - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.forward_parking_velocity, 0.0); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.forward_parking_velocity, 0.0); } else { - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.backward_parking_velocity, 0.0)); - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.backward_parking_velocity, 0.0); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.backward_parking_velocity, 0.0); } // set pull_over start and end pose diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 58fd392988ad9..97eba861f658a 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/utils.hpp" #include +#include namespace behavior_path_planner::utils::parking_departure { @@ -125,7 +126,7 @@ std::pair getPairsTerminalVelocityAndAccel( std::optional generateFeasibleStopPath( PathWithLaneId & current_path, std::shared_ptr planner_data, - geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + std::optional & stop_pose, const double maximum_deceleration, const double maximum_jerk) { if (current_path.points.empty()) { diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index f5d76f5cd4d35..bf93e71ab3591 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -514,8 +514,6 @@ std::vector getCollidedPolygons( const auto interpolated_data = getInterpolatedPoseWithVelocityAndPolygonStamped( predicted_ego_path, current_time, ego_vehicle_info); if (!interpolated_data) { - debug.expected_obj_pose = obj_pose; - debug.extended_obj_polygon = obj_polygon; continue; } const auto & ego_pose = interpolated_data->pose; diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index 994a7ba1a19d5..1d51426793de8 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline( return motion_utils::resamplePath(path, s_out); } -Path toPath(const PathWithLaneId & input) -{ - Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc) { @@ -186,16 +173,15 @@ size_t getIdxByArclength( } } return path.points.size() - 1; - } else { - for (size_t i = target_idx; i > 0; --i) { - const auto next_i = i - 1; - sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); - if (sum_length < signed_arc) { - return next_i; - } + } + for (size_t i = target_idx; i > 0; --i) { + const auto next_i = i - 1; + sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); + if (sum_length < signed_arc) { + return next_i; } - return 0; } + return 0; } // TODO(murooka) This function should be replaced with motion_utils::cropPoints @@ -642,8 +628,8 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); BehaviorModuleOutput output; - output.path = std::make_shared(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; @@ -692,8 +678,8 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 850919f539e59..ecd02bfd63485 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -14,63 +14,12 @@ #include #include +#include namespace behavior_path_planner::utils::traffic_light { using motion_utils::calcSignedArcLength; -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { - return false; - } - - const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { @@ -138,7 +87,8 @@ std::optional calcDistanceToRedTrafficLight( continue; } - if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + if (!traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { continue; } @@ -177,4 +127,24 @@ bool isStoppedAtRedTrafficLightWithinDistance( return (distance_to_red_traffic_light < distance_threshold); } +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { + return true; + } + } + } + + return false; +} + } // namespace behavior_path_planner::utils::traffic_light diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 7c3a5883989fb..e6f9ce21cebf7 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -22,6 +21,7 @@ #include #include #include +#include #include #include @@ -74,7 +74,7 @@ namespace behavior_path_planner::utils using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; -using tf2::fromMsg; +using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; std::optional getPolygonByPoint( diff --git a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 2cbaaf46e78cb..77728cc87604d 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -58,7 +58,7 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) Point2d query(5.0, -5.0); Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); - EXPECT_NEAR(projection.distance, -5.0, eps); + EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } @@ -82,7 +82,7 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) Point2d query(0.0, 0.0); Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); - EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); + EXPECT_NEAR(projection.distance, std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } @@ -115,7 +115,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); - EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); + EXPECT_NEAR(projection.distance, std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.projected_point.x(), 2.5, eps); EXPECT_NEAR(projection.projected_point.y(), 7.5, eps); } diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design.md b/planning/behavior_path_side_shift_module/README.md similarity index 100% rename from planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design.md rename to planning/behavior_path_side_shift_module/README.md diff --git a/planning/behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp index 69df77672cd96..6df8c1ec629c9 100644 --- a/planning/behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_side_shift_module/src/scene.cpp @@ -203,17 +203,17 @@ void SideShiftModule::updateData() ? planner_data_->self_odometry->pose.pose : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); if (prev_reference_.points.empty()) { - prev_reference_ = *getPreviousModuleOutput().path; + prev_reference_ = getPreviousModuleOutput().path; } - if (!getPreviousModuleOutput().reference_path) { + if (getPreviousModuleOutput().reference_path.points.empty()) { return; } const auto centerline_path = utils::calcCenterLinePath( planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); + getPreviousModuleOutput().reference_path); constexpr double resample_interval = 1.0; - const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path); + const auto backward_extened_path = extendBackwardLength(getPreviousModuleOutput().path); reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval); path_shifter_.setPath(reference_path_); @@ -286,7 +286,7 @@ BehaviorModuleOutput SideShiftModule::plan() output.reference_path = getPreviousModuleOutput().reference_path; prev_output_ = shifted_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); debug_data_.path_shifter = std::make_shared(path_shifter_); @@ -329,7 +329,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); prev_output_ = shifted_path; @@ -409,7 +409,8 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat { // for new architecture // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be // assigned without combining. - out.path = std::make_shared(output_path); + out.path = output_path; + out.reference_path = getPreviousModuleOutput().reference_path; out.drivable_area_info.drivable_lanes = expanded_lanes; out.drivable_area_info.is_already_expanded = true; } diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index 94be083bdebcd..eb7d1afe27549 100644 --- a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_start_planner_module/README.md similarity index 78% rename from planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md rename to planning/behavior_path_start_planner_module/README.md index 28a1db649cf04..aeefd18357a5c 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -2,17 +2,23 @@ ## Purpose / Role -The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.) -This module is activated when a new route is received. +The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected. -Use cases are as follows +Use cases include: -- start smoothly from the current ego position to centerline. - ![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) + +
+ ![case1](images/start_from_road_side.drawio.svg){width=1000} +
pull out from side of the road lane
+
+ - pull out from the shoulder lane to the road lane centerline. - ![case3](../image/start_from_road_shoulder.drawio.svg) + +
+ ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000} +
pull out from the shoulder lane
+
## Design @@ -59,18 +65,19 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles @@ -78,14 +85,14 @@ PullOutPath --o PullOutPlannerBase 2. Calculate object's polygon 3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path -![pull_out_collision_check](../image/pull_out_collision_check.drawio.svg) +![pull_out_collision_check](./images/pull_out_collision_check.drawio.svg) ## Safety check with dynamic obstacles ### **Basic concept of safety check against dynamic obstacles** This is based on the concept of RSS. For the logic used, refer to the link below. -See [safety check feature explanation](../docs/behavior_path_planner_safety_check.md) +See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) ### **Collision check performed range** @@ -103,7 +110,7 @@ Since there's a stop at the midpoint during the shift, this becomes the endpoint During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends. -![collision_check_range](../image/start_planner/collision_check_range.drawio.svg) +![collision_check_range](./images/collision_check_range.drawio.svg) ### **Ego vehicle's velocity planning** @@ -198,7 +205,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral - In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) - Combine this path with center line of road lane -![shift_pull_out](../image/shift_pull_out.drawio.svg) +![shift_pull_out](./images/shift_pull_out.drawio.svg) [shift pull out video](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) @@ -220,7 +227,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. -![geometric_pull_out](../image/geometric_pull_out.drawio.svg) +![geometric_pull_out](./images/geometric_pull_out.drawio.svg) [geometric pull out video](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) @@ -239,10 +246,56 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). -![pull_out_after_back](../image/pull_out_after_back.drawio.svg) +![pull_out_after_back](./images/pull_out_after_back.drawio.svg) [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) +### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +#### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +#### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + ### **parameters for backward pull out start point search** | Name | Unit | Type | Description | Default value | @@ -257,7 +310,7 @@ If a safe path cannot be generated from the current position, search backwards f ### **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` +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` @@ -276,4 +329,4 @@ To run this feature, you need to set `parking_lot` to the map, `activate_by_scen | 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. +See [freespace_planner](../freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 514d61e225ecd..d174b54b9ccbe 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,8 +5,9 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margin: 1.0 + collision_check_margins: [2.0, 1.5, 1.0] collision_check_distance_from_end: 1.0 + collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 diff --git a/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg b/planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg rename to planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg diff --git a/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg b/planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/geometric_pull_out.drawio.svg rename to planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg new file mode 100644 index 0000000000000..ab48c30b6fdae --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg @@ -0,0 +1,319 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + (0, shift_pull_out) +
+ + (1, shift_pull_out) +
+ (2, shift_pull_out) +
+ (3, shift_pull_out) +
+ (4, shift_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (0, + geometric_pull_out + ) +
+ (1, + geometric_pull_out + ) +
+ (2, + geometric_pull_out + ) +
+ (3, + geometric_pull_out + ) +
+ (4, + geometric_pull_out + ) +
+ ︙ +
+ (N, + geometric_pull_out + ) + +
+ +
+
+ + +
+
backward_search_resolution
+
+ + +
+
geometric pull out path
+
+ + +
+
start pose candidate
+
+ + +
+
idx: 0
+
+ + +
+
idx: 1
+
+ + +
+
idx: 2
+
+ + +
+
idx: 3
+
+ + +
+
idx: 4
+
+ + +
+
shift pull out path
+
+ + +
+
+ (idx, planner_type):= +
+ (3, geometric_pull_out) +
+
+ + +
+
(3, shift_pull_out)
+
+ + +
+
idx: N
+
+ + +
+
+ + efficient_path +
+ +
+
+ + +
+
+ (0, shift_pull_out) +
+ (0, geometric_pull_out) +
+ (1, shift_pull_out) +
+ (1, geometric_pull_out) +
+ (2, shift_pull_out) +
+ (2, geometric_pull_out) +
+ (3, shift_pull_out) +
+ (3, geometric_pull_out) +
+ (4, shift_pull_out) +
+ (4, geometric_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (N, geometric_pull_out) + +
+ +
+
+ + +
+
+ + short_back_distance +
+ +
+
+ + +
+
high priority
+
+ + +
+
low priority
+
+ + diff --git a/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg b/planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/pull_out_after_back.drawio.svg rename to planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg diff --git a/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg b/planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg rename to planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg diff --git a/planning/behavior_path_planner/image/shift_pull_out.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/shift_pull_out.drawio.svg rename to planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg diff --git a/planning/behavior_path_planner/image/start_from_road_shoulder.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_from_road_shoulder.drawio.svg rename to planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg diff --git a/planning/behavior_path_planner/image/start_from_road_side.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_from_road_side.drawio.svg rename to planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index de3d376fa4b3d..e345e3dc91337 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/start_planner_parameters.hpp" @@ -45,7 +44,7 @@ class PullOutPlannerBase explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 9012504ed2e7a..2d50ae189dc13 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -46,6 +46,10 @@ class ShiftPullOut : public PullOutPlannerBase double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); + bool refineShiftedPathToStartPose( + ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, + const double longitudinal_acc, const double lateral_acc); + std::shared_ptr lane_departure_checker_; private: diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a143f34ead649..41a80e59d56bf 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -68,7 +68,7 @@ struct PullOutStatus 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}; + std::optional stop_pose{std::nullopt}; PullOutStatus() {} }; @@ -169,11 +169,12 @@ class StartPlannerModule : public SceneModuleInterface bool isMoving() const; PriorityOrder determinePriorityOrder( - const std::string & search_priority, const size_t candidates_size); + const std::string & search_priority, const size_t start_pose_candidates_num); bool findPullOutPath( - const std::vector & start_pose_candidates, const size_t index, - const std::shared_ptr & planner, const Pose & refined_start_pose, - const Pose & goal_pose); + const Pose & start_pose_candidate, const std::shared_ptr & planner, + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); + + PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); @@ -227,7 +228,9 @@ class StartPlannerModule : public SceneModuleInterface void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, + const double velocity_threshold, const double object_check_backward_distance, + const double object_check_forward_distance) const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 783aace0983ca..66dbddebf99bb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -42,8 +42,9 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; - double collision_check_margin{0.0}; + std::vector collision_check_margins{}; double collision_check_distance_from_end{0.0}; + double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index 7d6dd60398e8e..d9ff155f36e6c 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -63,17 +63,7 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - 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::isPolygonOverlapLanelet); - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - return {}; - } const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..8cc0b34082e44 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,9 +43,12 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); - p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 73952ca02f558..51673410199b8 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -46,7 +46,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; - const auto & dynamic_objects = planner_data_->dynamic_object; const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; @@ -64,19 +63,12 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return std::nullopt; } - // 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::isPolygonOverlapLanelet); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); - // get safe path for (auto & pull_out_path : pull_out_paths) { auto & shift_path = pull_out_path.partial_paths.front(); // shift path is not separate but only one. - // check lane_departure and collision with path between pull_out_start to pull_out_end + // check lane_departure with path between pull_out_start to pull_out_end PathWithLaneId path_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); @@ -143,13 +135,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos continue; } - // check collision - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - continue; - } - shift_path.header = planner_data_->route_handler->getRouteHeader(); return pull_out_path; @@ -158,6 +143,58 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return std::nullopt; } +bool ShiftPullOut::refineShiftedPathToStartPose( + ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, + const double longitudinal_acc, const double lateral_acc) +{ + constexpr double TOLERANCE = 0.01; + constexpr size_t MAX_ITERATION = 100; + + // Lambda to check if change is above tolerance + auto is_within_tolerance = + [](const auto & prev_val, const auto & current_val, const auto & tolerance) { + return std::abs(current_val - prev_val) < tolerance; + }; + + size_t iteration = 0; + while (iteration < MAX_ITERATION) { + const double lateral_offset = + motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); + + PathShifter path_shifter; + path_shifter.setPath(shifted_path.path); + ShiftLine shift_line{}; + shift_line.start = start_pose; + shift_line.end = end_pose; + shift_line.end_shift_length = lateral_offset; + path_shifter.addShiftLine(shift_line); + path_shifter.setVelocity(0.0); + path_shifter.setLongitudinalAcceleration(longitudinal_acc); + path_shifter.setLateralAccelerationLimit(lateral_acc); + + if (!path_shifter.generate(&shifted_path, false)) { + RCLCPP_WARN( + rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"), + "Failed to generate shifted path"); + return false; + } + + if (is_within_tolerance( + lateral_offset, + motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), + TOLERANCE)) { + return true; + } + + iteration++; + } + + RCLCPP_WARN( + rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"), + "Failed to converge lateral offset until max iteration"); + return false; +} + std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose) @@ -303,6 +340,8 @@ std::vector ShiftPullOut::calcPullOutPaths( if (!path_shifter.generate(&shifted_path, offset_back)) { continue; } + refineShiftedPathToStartPose( + shifted_path, start_pose, *shift_end_pose_ptr, longitudinal_acc, lateral_acc); // set velocity const size_t pull_out_end_idx = diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 0d3892cde8023..d523f57125679 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -14,7 +14,6 @@ #include "behavior_path_start_planner_module/start_planner_module.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" @@ -371,11 +370,11 @@ BehaviorModuleOutput StartPlannerModule::plan() incrementPathIndex(); } - if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.stop_pose) { auto current_path = getCurrentPath(); const auto stop_path = behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); // Insert stop point in the path if needed @@ -384,17 +383,18 @@ BehaviorModuleOutput StartPlannerModule::plan() 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; + status_.stop_pose = stop_pose_; } else { path = current_path; } - } else if (!isWaitingApproval() && status_.has_stop_point) { + } else if (!isWaitingApproval() && status_.stop_pose) { // Delete stop point if conditions are met if (status_.is_safe_dynamic_objects && isStopped()) { - status_.has_stop_point = false; + status_.stop_pose = std::nullopt; path = getCurrentPath(); } path = *status_.prev_stop_path_after_approval; + stop_pose_ = status_.stop_pose; } else { path = getCurrentPath(); } @@ -402,11 +402,11 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - output.path = std::make_shared(path); + output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -512,11 +512,11 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - output.path = std::make_shared(stop_path); + output.path = stop_path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_unique(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -584,26 +584,33 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose)) - return; + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin)) { + start_planner_data_.selected_start_pose_candidate_index = index; + start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + return; + } + } } updateStatusIfNoSafePathFound(); } PriorityOrder StartPlannerModule::determinePriorityOrder( - const std::string & search_priority, const size_t candidates_size) + const std::string & search_priority, const size_t start_pose_candidates_num) { PriorityOrder order_priority; if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { - for (size_t i = 0; i < candidates_size; i++) { + for (size_t i = 0; i < start_pose_candidates_num; i++) { order_priority.emplace_back(i, planner); } } } else if (search_priority == "short_back_distance") { - for (size_t i = 0; i < candidates_size; i++) { + for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } @@ -616,46 +623,74 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( } bool StartPlannerModule::findPullOutPath( - const std::vector & start_pose_candidates, const size_t index, - const std::shared_ptr & planner, const Pose & refined_start_pose, - const Pose & goal_pose) + const Pose & start_pose_candidate, const std::shared_ptr & planner, + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { - // Ensure the index is within the bounds of the start_pose_candidates vector - if (index >= start_pose_candidates.size()) return false; + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + const auto & vehicle_footprint = vehicle_info_.createFootprint(); + // extract stop objects in pull out lane for collision check + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *dynamic_objects, 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::isPolygonOverlapLanelet); - const Pose & pull_out_start_pose = start_pose_candidates.at(index); - const bool is_driving_forward = - tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + // if start_pose_candidate is far from refined_start_pose, backward driving is necessary + const bool backward_is_unnecessary = + tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01; planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose); // If no path is found, return false if (!pull_out_path) { return false; } - // If driving forward, update status with the current path and return true - if (is_driving_forward) { - updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType()); - return true; + // check collision + if (utils::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, + collision_check_margin)) { + return false; } - // If this is the last start pose candidate, return false - if (index == start_pose_candidates.size() - 1) return false; - - const Pose & next_pull_out_start_pose = start_pose_candidates.at(index + 1); - const auto next_pull_out_path = planner->plan(next_pull_out_start_pose, goal_pose); + if (backward_is_unnecessary) { + updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); + return true; + } - // If no next path is found, return false - if (!next_pull_out_path) return false; + updateStatusWithNextPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); - // Update status with the next path and return true - updateStatusWithNextPath( - *next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType()); return true; } +PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +{ + PathWithLaneId combined_path; + for (const auto & partial_path : path.partial_paths) { + combined_path.points.insert( + combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); + } + // calculate collision check end idx + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + + if (collision_check_end_pose) { + return motion_utils::findNearestIndex( + combined_path.points, collision_check_end_pose->position); + } else { + return combined_path.points.size() - 1; + } + }); + // remove the point behind of collision check end pose + combined_path.points.erase( + combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); + return combined_path; +} + void StartPlannerModule::updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type) @@ -775,13 +810,9 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { - const bool has_received_new_route = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route) { + if (!receivedNewRoute()) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -812,9 +843,13 @@ void StartPlannerModule::updatePullOutStatus() return {*refined_start_pose}; }); - planWithPriority( - start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + if (!status_.backward_driving_complete) { + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + } + start_planner_data_.refined_start_pose = *refined_start_pose; + start_planner_data_.start_pose_candidates = start_pose_candidates; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -871,12 +906,19 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( { std::vector pull_out_start_pose_candidates{}; const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + + const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, + backward_path_length, std::numeric_limits::max()); - const auto stop_objects_in_pull_out_lanes = - filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity); + const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + std::numeric_limits::max()); // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. @@ -890,9 +932,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( back_path_from_start_pose.points, start_pose.position, -back_distance); - if (!backed_pose) { + if (!backed_pose) continue; + + if (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes, + parameters_->collision_check_margin_from_front_object)) continue; - } const double backed_pose_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length; @@ -909,7 +954,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if (utils::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margin)) { + parameters_->collision_check_margins.back())) { break; // poses behind this has a collision, so break. } @@ -919,16 +964,25 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( } PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const { const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); - // filter for objects located in pull_out_lanes and moving at a speed below the threshold - const auto [stop_objects_in_pull_out_lanes, others] = + // filter for objects located in pull out lanes and moving at a speed below the threshold + auto [stop_objects_in_pull_out_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + const auto path = planner_data_->route_handler->getCenterLinePath( + pull_out_lanes, object_check_backward_distance, object_check_forward_distance); + + utils::path_safety_checker::filterObjectsByPosition( + stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, + object_check_backward_distance); + return stop_objects_in_pull_out_lanes; } @@ -1182,7 +1236,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; const PathWithLaneId stop_path = generateStopPath(); - output.path = std::make_shared(stop_path); + output.path = stop_path; setDrivableAreaInfo(output); @@ -1201,7 +1255,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() } path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return output; } @@ -1258,7 +1312,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(*output.path), + output.path, generateDrivableLanes(output.path), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; @@ -1273,6 +1327,8 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::addFootprintMarker; + using marker_utils::createFootprintMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -1283,6 +1339,7 @@ void StartPlannerModule::setDebugData() const using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; + using visualization_msgs::msg::Marker; const auto life_time = rclcpp::Duration::from_seconds(1.5); auto add = [&](MarkerArray added) { @@ -1296,9 +1353,68 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); + add(createFootprintMarkerArray( + start_planner_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); + // visualize collision_check_end_pose and footprint + { + const auto local_footprint = vehicle_info_.createFootprint(); + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + getFullPath().points, status_.pull_out_path.end_pose.position, + parameters_->collision_check_distance_from_end); + if (collision_check_end_pose) { + add(createPoseMarkerArray( + *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0, + Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + const auto footprint = transformVector( + local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose)); + const double ego_z = planner_data_->self_odometry->pose.pose.position.z; + for (size_t i = 0; i < footprint.size(); i++) { + const auto & current_point = footprint.at(i); + const auto & next_point = footprint.at((i + 1) % footprint.size()); + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + } + marker.lifetime = life_time; + debug_marker_.markers.push_back(marker); + } + } + // start pose candidates + { + MarkerArray start_pose_footprint_marker_array{}; + MarkerArray start_pose_text_marker_array{}; + const auto purple = createMarkerColor(1.0, 0.0, 1.0, 0.99); + Marker footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), purple); + Marker text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates_idx", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); + footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + for (size_t i = 0; i < start_planner_data_.start_pose_candidates.size(); ++i) { + footprint_marker.id = i; + text_marker.id = i; + footprint_marker.points.clear(); + text_marker.text = "idx[" + std::to_string(i) + "]"; + text_marker.pose = start_planner_data_.start_pose_candidates.at(i); + addFootprintMarker( + footprint_marker, start_planner_data_.start_pose_candidates.at(i), vehicle_info_); + start_pose_footprint_marker_array.markers.push_back(footprint_marker); + start_pose_text_marker_array.markers.push_back(text_marker); + } + + add(start_pose_footprint_marker_array); + add(start_pose_text_marker_array); + } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (start_planner_data_.ego_predicted_path.size() > 0) { @@ -1389,7 +1505,7 @@ void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const status_.prev_is_safe_dynamic_objects ? "true" : "false"); logFunc(" Driving Forward: %s", status_.driving_forward ? "true" : "false"); logFunc(" Backward Driving Complete: %s", status_.backward_driving_complete ? "true" : "false"); - logFunc(" Has Stop Point: %s", status_.has_stop_point ? "true" : "false"); + logFunc(" Has Stop Pose: %s", status_.stop_pose ? "true" : "false"); logFunc("[Module State]"); logFunc(" isActivated: %s", isActivated() ? "true" : "false"); diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index 9a9466936c422..b34398083a95c 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -14,7 +14,6 @@ #include "behavior_path_start_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -92,7 +91,6 @@ lanelet::ConstLanelets getPullOutLanes( const auto start_pose = planner_data->route_handler->getOriginalStartPose(); lanelet::ConstLanelet current_shoulder_lane; - lanelet::ConstLanelets shoulder_lanes; if (route_handler->getPullOutStartLane( route_handler->getShoulderLanelets(), start_pose, vehicle_width, ¤t_shoulder_lane)) { // pull out from shoulder lane diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 9485f165849f2..1148379c26f42 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -21,7 +21,6 @@ behavior_velocity_planner_common geometry_msgs lanelet2_extension - libboost-dev motion_utils pluginlib rclcpp diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 429f6b5a76550..62a5e06674d4e 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -27,12 +27,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -203,7 +205,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return true; } -boost::optional BlindSpotModule::getFirstPointConflictingLanelets( +std::optional BlindSpotModule::getFirstPointConflictingLanelets( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & lanelets) const { @@ -229,7 +231,7 @@ boost::optional BlindSpotModule::getFirstPointConflictingLanelets( if (is_conflict) { return first_idx_conflicting_lanelets; } else { - return boost::none; + return std::nullopt; } } @@ -252,7 +254,7 @@ bool BlindSpotModule::generateStopLine( /* generate stop point */ int stop_idx_ip = 0; // stop point index for interpolated path. if (straight_lanelets.size() > 0) { - boost::optional first_idx_conflicting_lane_opt = + std::optional first_idx_conflicting_lane_opt = getFirstPointConflictingLanelets(path_ip, straight_lanelets); if (!first_idx_conflicting_lane_opt) { RCLCPP_DEBUG(logger_, "No conflicting line found."); @@ -261,7 +263,7 @@ bool BlindSpotModule::generateStopLine( stop_idx_ip = std::max( first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); } else { - boost::optional intersection_enter_point_opt = + std::optional intersection_enter_point_opt = getStartPointFromLaneLet(lane_id_); if (!intersection_enter_point_opt) { RCLCPP_DEBUG(logger_, "No intersection enter point found."); @@ -502,7 +504,7 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } -boost::optional BlindSpotModule::generateBlindSpotPolygons( +std::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, @@ -537,11 +539,21 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::ConstLanelets adjacent_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); - const auto adj = - turn_direction_ == TurnDirection::LEFT - ? (routing_graph_ptr->adjacentLeft(lane)) - : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) - : boost::none); + const auto adj = std::invoke([&]() -> std::optional { + if (turn_direction_ == TurnDirection::INVALID) { + return std::nullopt; + } + const auto adj_lane = (turn_direction_ == TurnDirection::LEFT) + ? routing_graph_ptr->adjacentLeft(lane) + : routing_graph_ptr->adjacentRight(lane); + + if (adj_lane) { + return *adj_lane; + } + + return std::nullopt; + }); + if (adj) { const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); @@ -577,7 +589,7 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( } return blind_spot_polygons; } else { - return boost::none; + return std::nullopt; } } @@ -628,13 +640,13 @@ bool BlindSpotModule::isTargetObjectType( return false; } -boost::optional BlindSpotModule::getStartPointFromLaneLet( +std::optional BlindSpotModule::getStartPointFromLaneLet( const lanelet::Id lane_id) const { lanelet::ConstLanelet lanelet = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); if (lanelet.centerline().empty()) { - return boost::none; + return std::nullopt; } const auto p = lanelet.centerline().front(); geometry_msgs::msg::Point start_point; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index bb78ed5771d2b..3915acd3532b5 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -25,8 +25,6 @@ #include #include -#include - #include #include @@ -128,7 +126,7 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return Blind spot polygons */ - boost::optional generateBlindSpotPolygons( + std::optional generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, @@ -192,7 +190,7 @@ class BlindSpotModule : public SceneModuleInterface * @param lanelets target lanelets * @return path point index */ - boost::optional getFirstPointConflictingLanelets( + std::optional getFirstPointConflictingLanelets( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & lanelets) const; @@ -201,8 +199,7 @@ class BlindSpotModule : public SceneModuleInterface * @param lane_id lane id of objective lanelet * @return end point of lanelet */ - boost::optional getStartPointFromLaneLet( - const lanelet::Id lane_id) const; + std::optional getStartPointFromLaneLet(const lanelet::Id lane_id) const; /** * @brief get straight lanelets in intersection diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 6559f39278fdc..273db320f1027 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -16,7 +16,7 @@ # For the case where the crosswalk width is very wide far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. - stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: @@ -38,7 +38,7 @@ # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) @@ -62,7 +62,8 @@ # param for target object filtering object_filtering: - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects bicycle: true # [-] whether to look and stop by BICYCLE objects diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 3f4af4a5a1138..77de7d241c05c 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -50,6 +50,8 @@ enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; struct CollisionPoint { geometry_msgs::msg::Point collision_point{}; + std::optional crosswalk_passage_direction{ + std::nullopt}; // denote obj is passing the crosswalk along the vehicle lane double time_to_collision{}; double time_to_vehicle{}; }; diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 3830aa9edddff..9eef11e72239e 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -10,6 +10,8 @@ Shumpei Wakabayashi Takayuki Murooka Kyoichi Sugahara + Mamoru Sobue + Yuki Takagi Apache License 2.0 diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index cb0311ffe8ebb..b8190cb6124e7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -116,6 +116,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for target area & object cp.crosswalk_attention_range = getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter( + node, ns + ".object_filtering.vehicle_object_cross_angle_threshold"); cp.look_unknown = getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); cp.look_bicycle = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 239eddca1fed9..acb88a214b0c0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -316,6 +317,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -349,25 +352,38 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop + auto isVehicleType = [](const uint8_t label) { + return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; + }; std::optional> nearest_stop_info; std::vector stop_factor_points; for (const auto & object : object_info_manager_.getObject()) { - const auto & collision_point = object.collision_point; - if (collision_point) { + const auto & collision_point_opt = object.collision_point; + if (collision_point_opt) { + const auto & collision_point = collision_point_opt.value(); const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } + if ( + isVehicleType(object.classification) && ego_crosswalk_passage_direction && + collision_point.crosswalk_passage_direction) { + const double direction_diff = tier4_autoware_utils::normalizeRadian( + collision_point.crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value()); + if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { + continue; + } + } stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength( - sparse_resample_path.points, ego_pos, collision_point->collision_point) - + calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { nearest_stop_info = - std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); + std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front); } } } @@ -377,7 +393,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return {}; } - // Check if the ego should stop beyond the stop line. + // Check if the ego should stop at the stop line or the other points. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; @@ -388,9 +404,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return createStopFactor(*default_stop_pose, stop_factor_points); } } else { - // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); } @@ -580,6 +596,70 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } +std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( + const PathWithLaneId & path) const +{ + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.points.size() - 1; ++i) { + const auto & start = path.points.at(i).point.pose.position; + const auto & end = path.points.at(i + 1).point.pose.position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + +std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const +{ + using tier4_autoware_utils::Segment2d; + + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.path.size() - 1; ++i) { + const auto & start = path.path.at(i).position; + const auto & end = path.path.at(i + 1).position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + std::optional CrosswalkModule::getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) @@ -625,6 +705,8 @@ std::optional CrosswalkModule::getCollisionPoint( // Calculate multi-step object polygon, and reset start_idx const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto object_crosswalk_passage_direction = + findObjectPassageDirectionAlongVehicleLane(obj_path); const auto obj_multi_step_polygon = createMultiStepPolygon( obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); is_start_idx_initialized = false; @@ -660,7 +742,8 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel, + object_crosswalk_passage_direction); debug_data_.obj_polygons.push_back( toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } @@ -679,19 +762,23 @@ std::optional CrosswalkModule::getCollisionPoint( CollisionPoint CrosswalkModule::createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const { constexpr double min_ego_velocity = 1.38; // [m/s] - const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity); CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; + collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction; + + // The decision of whether the ego vehicle or the pedestrian goes first is determined by the logic + // for ego_pass_first or yield; even the decision for ego_pass_later does not affect this sense. + // Hence, here, we use the length that would be appropriate for the ego_pass_first judge. collision_point.time_to_collision = - std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / + std::max(0.0, dist_ego2cp - planner_data_->vehicle_info_.min_longitudinal_offset_m) / std::max(ego_vel.x, min_ego_velocity); collision_point.time_to_vehicle = std::max(0.0, dist_obj2cp) / velocity; @@ -789,7 +876,7 @@ Polygon2d CrosswalkModule::getAttentionArea( std::optional CrosswalkModule::checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, - const std::optional & stop_pose) const + const std::optional & stop_pose) { const auto & p = planner_param_; @@ -854,6 +941,10 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); + + // early return may not appropriate because the nearest in range object should be handled return createStopFactor(*feasible_stop_pose, {obj_pos}); } } @@ -931,13 +1022,32 @@ 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_, crosswalk_.polygon2d().basicPolygon()); + has_traffic_light, collision_point, object.classification.front().label, planner_param_, + crosswalk_.polygon2d().basicPolygon()); + const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); if (collision_point) { - const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); debug_data_.collision_points.push_back( std::make_tuple(obj_uuid, *collision_point, collision_state)); } + + const auto getLabelColor = [](const auto collision_state) { + if (collision_state == CollisionState::YIELD) { + return ColorName::RED; + } else if ( + collision_state == CollisionState::EGO_PASS_FIRST || + collision_state == CollisionState::EGO_PASS_LATER) { + return ColorName::GREEN; + } else if (collision_state == CollisionState::IGNORE) { + return ColorName::GRAY; + } else { + return ColorName::AMBER; + } + }; + + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, + getLabelColor(collision_state)); } object_info_manager_.finalize(); } @@ -948,19 +1058,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - const auto traffic_signal_stamped = + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); - if (!traffic_signal_stamped) { + if (!traffic_signal_stamped_opt) { continue; } + const auto traffic_signal_stamped = traffic_signal_stamped_opt.value(); if ( planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + (clock_->now() - traffic_signal_stamped.stamp).seconds()) { continue; } - const auto & lights = traffic_signal_stamped->signal.elements; + const auto & lights = traffic_signal_stamped.signal.elements; if (lights.empty()) { continue; } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index ef6d01fc43c23..247907ad2dc80 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -150,6 +150,7 @@ class CrosswalkModule : public SceneModuleInterface double traffic_light_state_timeout; // param for target area & object double crosswalk_attention_range; + double vehicle_object_cross_angle_threshold; bool look_unknown; bool look_bicycle; bool look_motorcycle; @@ -160,6 +161,7 @@ class CrosswalkModule : public SceneModuleInterface { CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; + uint8_t classification{ObjectClassification::UNKNOWN}; geometry_msgs::msg::Point position{}; std::optional collision_point{}; @@ -242,8 +244,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 lanelet::BasicPolygon2d & crosswalk_polygon) + const std::optional & collision_point, const uint8_t classification, + const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -265,6 +267,7 @@ class CrosswalkModule : public SceneModuleInterface crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; + objects.at(uuid).classification = classification; } void finalize() { @@ -328,7 +331,12 @@ class CrosswalkModule : public SceneModuleInterface std::optional checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, - const std::optional & stop_pose) const; + const std::optional & stop_pose); + + std::optional findEgoPassageDirectionAlongPath( + const PathWithLaneId & sparse_resample_path) const; + std::optional findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const; std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, @@ -366,7 +374,8 @@ class CrosswalkModule : public SceneModuleInterface CollisionPoint createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const; + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const; float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt new file mode 100644 index 0000000000000..20f4b3aa8f4d3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_dynamic_obstacle_stop_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md new file mode 100644 index 0000000000000..8aa3415c3f329 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md @@ -0,0 +1,85 @@ +## Dynamic Obstacle Stop + +### Role + +`dynamic_obstacle_stop` is a module that stops the ego vehicle from entering the _immediate_ path of a dynamic object. + +The _immediate_ path of an object is the area that the object would traverse during a given time horizon, assuming constant velocity and heading. + +![rviz example](docs/dynamic_obstacle_stop_rviz.png) + +### Activation Timing + +This module is activated if the launch parameter `launch_dynamic_obstacle_stop_module` is set to true in the behavior planning launch file. + +### Inner-workings / Algorithms + +The module insert a stop point where the ego path collides with the immediate path of an object. +The overall module flow can be summarized with the following 4 steps. + +1. Filter dynamic objects. +2. Calculate immediate path rectangles of the dynamic objects. +3. Find earliest collision where ego collides with an immediate path rectangle. +4. Insert stop point before the collision. + +In addition to these 4 steps, 2 mechanisms are in place to make the stop point of this module more stable: an hysteresis and a decision duration buffer. + +The `hysteresis` parameter is used when a stop point was already being inserted in the previous iteration +and it increases the range where dynamic objects are considered close enough to the ego path to be used by the module. + +The `decision_duration_buffer` parameter defines the duration when the module will keep inserted the previous stop point, even after no collisions were found. + +#### Filter dynamic objects + +![filtering example](docs/DynamicObstacleStop-Filtering.drawio.svg) + +An object is considered by the module only if it meets all of the following conditions: + +- it is a vehicle (pedestrians are ignored); +- it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; +- it is not too close to the current position of the ego vehicle; +- it is close to the ego path. + +For the last condition, +the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). +In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. + +#### Calculate immediate path rectangles + +![Immediate paths example](docs/DynamicObstacleStop-ImmediatePaths.drawio.svg) + +For each considered object, a rectangle is created representing its _immediate_ path. +The rectangle has the width of the object plus the `extra_object_width` parameter +and its length is the current speed of the object multiplied by the `time_horizon`. + +#### Find earliest collision + +![Earliest collision example](docs/DynamicObstacleStop-Collision.drawio.svg) + +We build the ego path footprints as the set of ego footprint polygons projected on each path point. +We then calculate the intersections between these ego path footprints and the previously calculated immediate path rectangles. +An intersection is ignored if the object is not driving toward ego, i.e., the absolute angle between the object and the path point is larger than $\frac{3 \pi}{4}$. + +The collision point with the lowest arc length when projected on the ego path will be used to calculate the final stop point. + +#### Insert stop point + +![Stop point example](docs/DynamicObstacleStop-StopPoint.drawio.svg) + +Before inserting a stop point, we calculate the range of path arc lengths where it can be inserted. +The minimum is calculated to satisfy the acceleration and jerk constraints of the vehicle. +If a stop point was inserted in the previous iteration of the module, its arc length is used as the maximum. +Finally, +the stop point arc length is calculated to be the arc length of the previously found collision point minus the `stop_distance_buffer` and the ego vehicle longitudinal offset, clamped between the minimum and maximum values. + +### Module Parameters + +| Parameter | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | +| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml new file mode 100644 index 0000000000000..14483093e8bb3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object + extra_object_width: 1.0 # [m] extra width around detected objects + minimum_object_velocity: 0.5 # [m/s] objects with a velocity bellow this value are ignored + stop_distance_buffer: 0.5 # [m] extra distance to add between the stop point and the collision point + time_horizon: 5.0 # [s] time horizon used for collision checks + hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection + decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled + minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg new file mode 100644 index 0000000000000..77beb27c20db8 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg @@ -0,0 +1,236 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + + + + + + + +
+
+
+
abs(angle) < ¾π
+
collisions are ignored
+
+
+
+ + abs(angle) < ¾π... + + + + + + + +
+
+
+ earliest collision point +
+
+
+ + earliest collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg new file mode 100644 index 0000000000000..08638931b599f --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg @@ -0,0 +1,156 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + +
+
+
+ NPC 2 +
+
+
+ + NPC 2 + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + +
+
+
+
NPC 4
+
+
+
+ + NPC 4 + + + + + + + + +
+
+
+ NPC 2 is too far from the path and is ignored +
+
+
+ + NPC 2 is too far from the path and is ig... + + + + + + + + + +
+
+
+ NPC 4 is too close to ego and is ignored +
+
+
+ + NPC 4 is too close to ego and is ignored + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg new file mode 100644 index 0000000000000..3f07c95c7399d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg @@ -0,0 +1,103 @@ + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + +
+
+
+
object width +
+
extra_object_width
+
+
+
+ + object width +... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg new file mode 100644 index 0000000000000..0b0953cbea265 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg @@ -0,0 +1,98 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + +
+
+
+ ego longitudinal offset + extra_distance_buffer +
+
+
+ + ego longitudinal offset + extra_distance_buffer + + + + + + + +
+
+
+ stop point +
+
+
+ + stop point + + + + + + +
+
+
+ collision point +
+
+
+ + collision... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png new file mode 100644 index 0000000000000..169cb40875d2d Binary files /dev/null and b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png differ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml new file mode 100644 index 0000000000000..7e9599c49bc2d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -0,0 +1,36 @@ + + + + behavior_velocity_dynamic_obstacle_stop_module + 0.1.0 + dynamic_obstacle_stop module to stop ego when in the immediate path of a dynamic object + + Maxime Clement + Mamoru Sobue + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..2f5662c7998ac --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp new file mode 100644 index 0000000000000..46c58d6be8805 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -0,0 +1,71 @@ +// 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 "collision.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data) +{ + auto earliest_idx = ego_data.path_footprints.size(); + auto earliest_arc_length = motion_utils::calcArcLength(ego_data.path.points); + std::optional earliest_collision_point; + debug_data.collisions.clear(); + std::vector rough_collisions; + for (auto obstacle_idx = 0UL; obstacle_idx < objects.size(); ++obstacle_idx) { + rough_collisions.clear(); + const auto & obstacle_pose = objects[obstacle_idx].kinematics.initial_pose_with_covariance.pose; + const auto & obstacle_footprint = obstacle_forward_footprints[obstacle_idx]; + ego_data.rtree.query( + boost::geometry::index::intersects(obstacle_footprint), std::back_inserter(rough_collisions)); + for (const auto & rough_collision : rough_collisions) { + const auto path_idx = rough_collision.second; + const auto & ego_footprint = ego_data.path_footprints[path_idx]; + const auto & ego_pose = ego_data.path.points[ego_data.first_path_idx + path_idx].point.pose; + const auto angle_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(ego_pose.orientation) - tf2::getYaw(obstacle_pose.orientation)); + if (path_idx <= earliest_idx && std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { + tier4_autoware_utils::MultiPoint2d collision_points; + boost::geometry::intersection( + obstacle_footprint.outer(), ego_footprint.outer(), collision_points); + earliest_idx = path_idx; + for (const auto & coll_p : collision_points) { + auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); + const auto arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, ego_data.first_path_idx, p); + if (arc_length < earliest_arc_length) { + earliest_arc_length = arc_length; + debug_data.collisions = {obstacle_footprint, ego_footprint}; + earliest_collision_point = p; + } + } + } + } + } + return earliest_collision_point; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp new file mode 100644 index 0000000000000..ccc3fa94df603 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -0,0 +1,39 @@ +// 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 COLLISION_HPP_ +#define COLLISION_HPP_ + +#include "types.hpp" + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief find the earliest collision along the ego path and an obstacle footprint +/// @param [in] ego_data ego data including its path and footprint +/// @param [in] objects obstacles +/// @param [in] obstacle_forward_footprints obstacle footprints +/// @param [in] debug_data debug data +/// @return the point of earliest collision along the ego path +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp new file mode 100644 index 0000000000000..cc5dafa218654 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -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. + +#include "debug.hpp" + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ + +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + marker.ns = ns; + for (marker.id = static_cast(from); marker.id < static_cast(to); ++marker.id) + markers.push_back(marker); + return markers; +} + +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "dynamic_obstacles"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 1.0); + marker.text = "dynamic obstacle"; + for (const auto & obstacle : obstacles) { + marker.pose = obstacle.kinematics.initial_pose_with_covariance.pose; + markers.push_back(marker); + ++marker.id; + } + return markers; +} + +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = ns; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + for (const auto & footprint : footprints) { + marker.points.clear(); + for (const auto & p : footprint.outer()) { + marker.points.emplace_back(); + marker.points.back().x = p.x(); + marker.points.back().y = p.y(); + marker.points.back().z = z; + } + markers.push_back(marker); + ++marker.id; + } + return markers; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp new file mode 100644 index 0000000000000..02f550d314d39 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -0,0 +1,40 @@ +// 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 DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns); +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles); +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); +std::vector make_collision_markers( + const tier4_autoware_utils::MultiPoint2d & collisions, const double z); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug + +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp new file mode 100644 index 0000000000000..6a0c61963ac81 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -0,0 +1,84 @@ +// 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 "footprint.hpp" + +#include + +#include + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis) +{ + tier4_autoware_utils::MultiPolygon2d forward_footprints; + for (const auto & obstacle : obstacles) + forward_footprints.push_back(project_to_pose( + make_forward_footprint(obstacle, params, hysteresis), + obstacle.kinematics.initial_pose_with_covariance.pose)); + return forward_footprints; +} + +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis) +{ + const auto & shape = obstacle.shape.dimensions; + const auto longitudinal_offset = + shape.x / 2.0 + + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; + const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; + return tier4_autoware_utils::Polygon2d{ + {{longitudinal_offset, -lateral_offset}, + {longitudinal_offset, lateral_offset}, + {shape.x / 2.0, lateral_offset}, + {shape.x / 2.0, -lateral_offset}, + {longitudinal_offset, -lateral_offset}}, + {}}; +} + +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + tier4_autoware_utils::Polygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) +{ + for (const auto & p : ego_data.path.points) + ego_data.path_footprints.push_back(tier4_autoware_utils::toFootprint( + p.point.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + for (auto i = 0UL; i < ego_data.path_footprints.size(); ++i) { + const auto box = + boost::geometry::return_envelope(ego_data.path_footprints[i]); + ego_data.rtree.insert(std::make_pair(box, i)); + } +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp new file mode 100644 index 0000000000000..c28e89ac6c9f6 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -0,0 +1,55 @@ +// 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 FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +/// @brief create the footprint of the given obstacles and their projection over a fixed time +/// horizon +/// @param [in] obstacles obstacles +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprints +/// @return forward footprint of the obstacle +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis); +/// @brief create the footprint of the given obstacle and its projection over a fixed time horizon +/// @param [in] obstacle obstacle +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprint +/// @return forward footprint of the obstacle +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief create the rtree indexing the ego footprint along the path +/// @param [inout] ego_data ego data with its path and the rtree to populate +/// @param [in] params parameters +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp new file mode 100644 index 0000000000000..99981007b20c2 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -0,0 +1,73 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include "scene_dynamic_obstacle_stop.hpp" + +#include + +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::getOrDeclareParameter; + +DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + + pp.extra_object_width = getOrDeclareParameter(node, ns + ".extra_object_width"); + pp.minimum_object_velocity = getOrDeclareParameter(node, ns + ".minimum_object_velocity"); + pp.stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); + pp.time_horizon = getOrDeclareParameter(node, ns + ".time_horizon"); + pp.hysteresis = getOrDeclareParameter(node, ns + ".hysteresis"); + pp.decision_duration_buffer = + getOrDeclareParameter(node, ns + ".decision_duration_buffer"); + pp.minimum_object_distance_from_ego_path = + getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); + + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.ego_lateral_offset = + std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); + pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; +} + +void DynamicObstacleStopModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) return; + // general + if (!isModuleRegistered(module_id_)) { + registerModule(std::make_shared( + module_id_, planner_param_, logger_.get_child("dynamic_obstacle_stop_module"), clock_)); + } +} + +std::function &)> +DynamicObstacleStopModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; + }; +} +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::DynamicObstacleStopModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp new file mode 100644 index 0000000000000..e461cc9c16445 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -0,0 +1,58 @@ +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_dynamic_obstacle_stop.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface +{ +public: + explicit DynamicObstacleStopModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "dynamic_obstacle_stop"; } + +private: + using PlannerParam = dynamic_obstacle_stop::PlannerParam; + + PlannerParam planner_param_; + int64_t module_id_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class DynamicObstacleStopModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp new file mode 100644 index 0000000000000..0411ab2d01cfe --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -0,0 +1,71 @@ +// 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 "object_filtering.hpp" + +#include "types.hpp" + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis) +{ + std::vector filtered_objects; + const auto is_vehicle = [](const auto & o) { + return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + }) != o.classification.end(); + }; + const auto is_in_range = [&](const auto & o) { + const auto distance = std::abs(motion_utils::calcLateralOffset( + ego_data.path.points, o.kinematics.initial_pose_with_covariance.pose.position)); + return distance <= params.minimum_object_distance_from_ego_path + params.ego_lateral_offset + + o.shape.dimensions.y / 2.0 + hysteresis; + }; + const auto is_not_too_close = [&](const auto & o) { + const auto obj_arc_length = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, + o.kinematics.initial_pose_with_covariance.pose.position); + return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; + }; + for (const auto & object : objects.objects) + if ( + is_vehicle(object) && + object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity && + is_in_range(object) && is_not_too_close(object)) + filtered_objects.push_back(object); + return filtered_objects; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp new file mode 100644 index 0000000000000..22857f6db1bda --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -0,0 +1,39 @@ +// 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 OBJECT_FILTERING_HPP_ +#define OBJECT_FILTERING_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp new file mode 100644 index 0000000000000..98b7984bab1dc --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -0,0 +1,185 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_dynamic_obstacle_stop.hpp" + +#include "collision.hpp" +#include "debug.hpp" +#include "footprint.hpp" +#include "object_filtering.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +DynamicObstacleStopModule::DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) +{ + prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type()); + velocity_factor_.init(PlanningBehavior::UNKNOWN); +} + +bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_.reset_data(); + *stop_reason = planning_utils::initializeStopReason(StopReason::OBSTACLE_STOP); + if (!path || path->points.size() < 2) return true; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + + stopwatch.tic("preprocessing"); + EgoData ego_data; + ego_data.pose = planner_data_->current_odometry->pose; + ego_data.path.points = path->points; + motion_utils::removeOverlapPoints(ego_data.path.points); + ego_data.first_path_idx = + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( + ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); + + make_ego_footprint_rtree(ego_data, params_); + const auto has_decided_to_stop = + (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; + if (!has_decided_to_stop) current_stop_pose_.reset(); + double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0; + const auto dynamic_obstacles = + filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_, hysteresis); + + const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); + + stopwatch.tic("footprints"); + const auto obstacle_forward_footprints = + make_forward_footprints(dynamic_obstacles, params_, hysteresis); + const auto footprints_duration_us = stopwatch.toc("footprints"); + const auto min_stop_distance = + motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, + planner_data_->max_stop_jerk_threshold) + .value_or(0.0); + stopwatch.tic("collisions"); + const auto collision = + find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_); + const auto collisions_duration_us = stopwatch.toc("collisions"); + if (collision) { + const auto arc_length_diff = + motion_utils::calcSignedArcLength(ego_data.path.points, *collision, ego_data.pose.position); + const auto can_stop_before_limit = arc_length_diff < min_stop_distance - + params_.ego_longitudinal_offset - + params_.stop_distance_buffer; + const auto stop_pose = can_stop_before_limit + ? motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, *collision, + -params_.stop_distance_buffer - params_.ego_longitudinal_offset) + : motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, ego_data.pose.position, min_stop_distance); + if (stop_pose) { + const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength( + path->points, stop_pose->position, + current_stop_pose_->position) > 0.0; + if (use_new_stop_pose) current_stop_pose_ = *stop_pose; + prev_stop_decision_time_ = clock_->now(); + } + } + + if (current_stop_pose_) motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points); + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " + "%2.2fus\n\tcollisions = %2.2fus\n", + total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); + debug_data_.ego_footprints = ego_data.path_footprints; + debug_data_.obstacle_footprints = obstacle_forward_footprints; + return true; +} + +MarkerArray DynamicObstacleStopModule::createDebugMarkerArray() +{ + constexpr auto z = 0.0; + MarkerArray debug_marker_array; + // dynamic obstacles footprints + const auto obstacle_footprint_markers = + debug::make_polygon_markers(debug_data_.obstacle_footprints, "dynamic_obstacles_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), obstacle_footprint_markers.begin(), + obstacle_footprint_markers.end()); + const auto delete_footprint_markers = debug::make_delete_markers( + obstacle_footprint_markers.size(), debug_data_.prev_dynamic_obstacles_nb, + "dynamic_obstacles_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_footprint_markers.begin(), + delete_footprint_markers.end()); + // ego path footprints + const auto ego_footprint_markers = + debug::make_polygon_markers(debug_data_.ego_footprints, "ego_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), ego_footprint_markers.begin(), ego_footprint_markers.end()); + const auto delete_ego_footprint_markers = debug::make_delete_markers( + ego_footprint_markers.size(), debug_data_.prev_ego_footprints_nb, "ego_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_ego_footprint_markers.begin(), + delete_ego_footprint_markers.end()); + // collisions + auto collision_markers = debug::make_polygon_markers(debug_data_.collisions, "collisions", z); + for (auto & m : collision_markers) m.color.r = 1.0; + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), collision_markers.begin(), collision_markers.end()); + const auto delete_collision_markers = debug::make_delete_markers( + collision_markers.size(), debug_data_.prev_collisions_nb, "collisions"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_collision_markers.begin(), + delete_collision_markers.end()); + + debug_data_.prev_dynamic_obstacles_nb = obstacle_footprint_markers.size(); + debug_data_.prev_collisions_nb = collision_markers.size(); + debug_data_.prev_ego_footprints_nb = ego_footprint_markers.size(); + return debug_marker_array; +} + +motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls{}; + if (current_stop_pose_) { + motion_utils::VirtualWall virtual_wall; + virtual_wall.text = "dynamic_obstacle_stop"; + virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; + virtual_wall.style = motion_utils::VirtualWallType::stop; + virtual_wall.pose = *current_stop_pose_; + virtual_walls.push_back(virtual_wall); + } + return virtual_walls; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp new file mode 100644 index 0000000000000..c7bca149890a0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -0,0 +1,62 @@ +// 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 SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ +#define SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ + +#include "types.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +class DynamicObstacleStopModule : public SceneModuleInterface +{ +public: + DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); + + /// @brief insert stop or slow down points to prevent dangerously entering another lane + /// @param [inout] path the path to update + /// @param [inout] stop_reason reason for stopping + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + motion_utils::VirtualWalls createVirtualWalls() override; + +private: + PlannerParam params_; + rclcpp::Time prev_stop_decision_time_; + std::optional current_stop_pose_; + +protected: + int64_t module_id_{}; + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp new file mode 100644 index 0000000000000..74fd5ca818fb0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -0,0 +1,80 @@ +// 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +typedef std::pair BoxIndexPair; +typedef boost::geometry::index::rtree> Rtree; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + double extra_object_width; + double minimum_object_velocity; + double stop_distance_buffer; + double time_horizon; + double hysteresis; + double decision_duration_buffer; + double ego_longitudinal_offset; + double ego_lateral_offset; + double minimum_object_distance_from_ego_path; +}; + +struct EgoData +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + size_t first_path_idx{}; + double longitudinal_offset_to_first_path_idx; // [m] + geometry_msgs::msg::Pose pose; + tier4_autoware_utils::MultiPolygon2d path_footprints; + Rtree rtree; +}; + +/// @brief debug data +struct DebugData +{ + tier4_autoware_utils::MultiPolygon2d obstacle_footprints{}; + size_t prev_dynamic_obstacles_nb{}; + tier4_autoware_utils::MultiPolygon2d collisions{}; + size_t prev_collisions_nb{}; + tier4_autoware_utils::MultiPolygon2d ego_footprints{}; + size_t prev_ego_footprints_nb{}; + std::optional stop_pose{}; + void reset_data() + { + obstacle_footprints.clear(); + collisions.clear(); + ego_footprints.clear(); + stop_pose.reset(); + } +}; + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index db26be11b1c9a..58c2ce59edd48 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -184,6 +184,12 @@ If there is any object on the path inside the intersection and at the exit of th ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) +## Yield stuck vehicle detection + +If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection. This scene happens when the object is yielding against ego or the object is waiting before the crosswalk around the exit of the intersection. + +![yield_stuck_detection](./docs/yield-stuck.drawio.svg) + ## Collision detection The following process is performed for the targets objects to determine whether ego can pass the intersection safely. If it is judged that ego cannot pass the intersection with enough margin, this module inserts a stopline on the path. diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 1e6ce843de528..4e9eb50f2a462 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -23,15 +23,14 @@ stuck_vehicle_velocity_threshold: 0.833 # enable_front_car_decel_prediction: false # assumed_front_car_decel: 1.0 - timeout_private_area: 3.0 - enable_private_area_stuck_disregard: false + disable_against_private_lane: true yield_stuck: turn_direction: left: true - right: false + right: true straight: false - distance_threshold: 1.0 + distance_threshold: 5.0 collision_detection: consider_wrong_direction_vehicle: false @@ -73,7 +72,7 @@ enable: false creep_velocity: 0.8333 peeking_offset: -0.5 - occlusion_required_clearance_distance: 55 + occlusion_required_clearance_distance: 55.0 possible_object_bbox: [1.5, 2.5] ignore_parked_vehicle_speed_threshold: 0.8333 occlusion_detection_hold_time: 1.5 diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg new file mode 100644 index 0000000000000..e0078540ba407 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg @@ -0,0 +1,750 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + w/ traffic light, right +
+
+ (Left-hand traffic) + + + +
+ + +
+
+
+ + w/ traffic light, right... + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention lane +
+
+
+ + attention lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + T-shape junction w/o traffic light +
+
+ Right-hand traffic +
+ +
+
+
+ + T-shape junction w/o traffic light... + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 8d9beb34d05ee..5f103e0ecd3b0 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.first_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( @@ -196,6 +212,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.yield_stuck_detect_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, + 0.34509, 0.6588235), + &debug_marker_array); + } + if (debug_data_.ego_lane) { appendMarkerArray( createLaneletPolygonsMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 789708abe98f8..c92f16dd7b474 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -74,10 +74,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_velocity_threshold = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); - 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.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); ip.yield_stuck.turn_direction.left = getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); @@ -154,6 +152,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); ip.occlusion.peeking_offset = getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); ip.occlusion.possible_object_bbox = getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = @@ -201,7 +201,6 @@ void IntersectionModuleManager::launchNewModules( } 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; @@ -213,7 +212,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, is_private_area, node_, + has_traffic_light, enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 0d483b501d1ee..dad0c194b5c9f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,11 @@ #include "util.hpp" #include +#include #include +#include +#include +#include #include #include #include @@ -37,10 +41,27 @@ #include #include +#include #include #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; @@ -71,6 +92,25 @@ Polygon2d createOneStepPolygon( } } // namespace +static bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -97,8 +137,8 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, 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) + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), @@ -107,7 +147,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), - is_private_area_(is_private_area), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -377,7 +416,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, 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) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -393,7 +432,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, - [[maybe_unused]] util::DebugData * debug_data) + [[maybe_unused]] DebugData * debug_data) { return; } @@ -404,7 +443,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::StuckStop & 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -452,7 +491,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::YieldStuckStop & 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -484,7 +523,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -527,7 +566,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, 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) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -578,7 +617,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & decision_result, 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) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -635,7 +674,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedCollisionStop & 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -682,7 +721,7 @@ void reactRTCApprovalByDecisionResult( 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -735,7 +774,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::Safe & 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -777,7 +816,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FullyPrioritized & 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) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -819,7 +858,7 @@ void reactRTCApproval( const IntersectionModule::DecisionResult & decision_result, 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) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { std::visit( VisitorSwitch{[&](const auto & decision) { @@ -869,7 +908,7 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - debug_data_ = util::DebugData(); + debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); // set default RTC @@ -901,12 +940,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } -static bool isGreenSolidOn( - lanelet::ConstLanelet lane, const std::map & tl_infos) +bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -915,12 +953,13 @@ static bool isGreenSolidOn( // 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()) { + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { // the info of this traffic light is not available return false; } - const auto & tl_info = tl_info_it->second; + const auto & tl_info = tl_info_opt.value(); for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -939,6 +978,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( @@ -953,26 +994,21 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // 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); if (!intersection_lanelets_) { - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.common.attention_area_length, - planner_param_.occlusion.occlusion_attention_area_length, - planner_param_.collision_detection.consider_wrong_direction_vehicle); + intersection_lanelets_ = + getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); } auto & intersection_lanelets = intersection_lanelets_.value(); - // at the very first time of registration of this module, the path may not be conflicting with the + // 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 auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); const bool is_prioritized = - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -983,43 +1019,42 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); + const auto & second_attention_area_opt = intersection_lanelets.second_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 auto & dummy_first_attention_lane_centerline = - intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value().centerline2d() - : first_conflicting_lane.centerline2d(); - const auto intersection_stoplines_opt = util::generateIntersectionStopLines( - first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, - planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.default_stopline_margin, planner_param_.common.max_accel, - planner_param_.common.max_jerk, planner_param_.common.delay_response_time, - planner_param_.occlusion.peeking_offset, path); + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, + interpolated_path_info, path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto - [closest_idx, stuck_stopline_idx_opt, default_stopline_idx_opt, - first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt, - default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; // see the doc for struct PathLanelets + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); 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.attention_area(), closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = generatePathLanelets( + lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, + first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } - const auto path_lanelets = path_lanelets_opt.value(); + const auto & path_lanelets = path_lanelets_opt.value(); // utility functions auto fromEgoDist = [&](const size_t index) { @@ -1055,30 +1090,39 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // 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_stopline_idx_opt) { - auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { - if ( - default_stopline_idx_opt && - fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) { - stuck_stopline_idx = default_stopline_idx_opt.value(); - } + const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); + if (stuck_detected) { + if ( + is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane) { + // do nothing } else { - return IntersectionModule::StuckStop{ - closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt}; + std::optional stopline_idx = std::nullopt; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = default_stopline_idx_opt.value(); + } else if ( + first_attention_stopline_idx_opt && + fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return IntersectionModule::StuckStop{ + closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + } } } - // yield stuck vehicle detection is viable even if attention area is empty - // so this needs to be checked before attention area validation - const bool yield_stuck_detected = checkYieldStuckVehicle( - planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); - if (yield_stuck_detected && stuck_stopline_idx_opt) { - const auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx}; - } - // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; @@ -1108,14 +1152,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.attention_area = intersection_lanelets.attention_area(); debug_data_.occlusion_attention_area = occlusion_attention_area; debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); // check occlusion on detection lane if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_divisions_ = generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution, - planner_param_.occlusion.attention_lane_crop_curvature_threshold, - planner_param_.occlusion.attention_lane_curvature_calculation_ds); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); @@ -1124,16 +1168,41 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const double is_amber_or_red = - (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), + &debug_data_); + if (yield_stuck_detected) { + std::optional stopline_idx = std::nullopt; + const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; + const bool is_before_first_attention_stopline = + fromEgoDist(first_attention_stopline_idx) >= 0.0; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (is_before_default_stopline) { + stopline_idx = default_stopline_idx; + } else if (is_before_first_attention_stopline) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()}; + } + } + + const bool is_amber_or_red = + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? getOcclusionStatus( - *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, - planner_param_.occlusion.occlusion_required_clearance_distance) + occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, + occlusion_attention_divisions, target_objects) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -1197,8 +1266,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // 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); + const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet); if (is_green_solid_on) { if (!initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); @@ -1351,9 +1419,704 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) +{ + 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 TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto & tl_info = tl_info_opt.value(); + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; + } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } + } + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; +} + +static std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +IntersectionLanelets IntersectionModule::getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + lanelet::ConstLanelets yield_lanelets{}; + const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); + for (const auto & right_of_way : right_of_ways) { + if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { + for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { + yield_lanelets.push_back(yield_lanelet); + for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { + yield_lanelets.push_back(previous_lanelet); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets = lanelets_on_path; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + ego_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { + continue; + } + ego_lanelets.push_back(following_lanelet); + } + } + + // get conflicting lanes on assigned lanelet + const auto & conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + std::vector adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = + std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); + + // find the index of the first point whose vehicle footprint on it intersects with attention_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const std::optional first_footprint_inside_1st_attention_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_1st_attention_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the attention area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); + } + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; + const bool first_attention_stopline_valid = true; + + // (5) 1st pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; + if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching attention area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + } + } + const auto second_attention_stopline_ip = + second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) + : 0; + + // (8) second pass judge lie position on interpolated path + int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + const auto second_pass_judge_line_ip = + static_cast(std::max(second_pass_judge_ip_int, 0)); + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; +} + +/** + * @brief Get stop point from map if exists + * @param stop_pose stop point defined on map + * @return true when the stop point is defined on map. + */ +std::optional IntersectionModule::getStopLineIndexFromMap( + const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; i++) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(extended_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +static lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +static lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval) { + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +static std::optional> +getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) +{ + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + PathLanelets path_lanelets; + // prev + path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const PathLanelets & path_lanelets, DebugData * debug_data) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + const bool stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || @@ -1363,26 +2126,147 @@ bool IntersectionModule::checkStuckVehicle( return false; } - const auto & objects_ptr = planner_data->predicted_objects; + const auto & objects_ptr = planner_data_->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path - const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - return util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, &debug_data_); + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area && debug_data) { + debug_data->stuck_targets.objects.push_back(object); + return true; + } + } + return false; } -bool IntersectionModule::checkYieldStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area) +static lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) { - if (!first_attention_area) { - return false; + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; } + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +static lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) +{ const bool yield_stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || @@ -1392,24 +2276,74 @@ bool IntersectionModule::checkYieldStuckVehicle( return false; } - const auto & objects_ptr = planner_data->predicted_objects; - - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - - return util::checkYieldStuckVehicleInIntersection( - objects_ptr, ego_poly, first_attention_area.value(), - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, - planner_param_.yield_stuck.distance_threshold, &debug_data_); + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data->yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; } -util::TargetObjects IntersectionModule::generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, +TargetObjects IntersectionModule::generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - util::TargetObjects target_objects; + TargetObjects target_objects; target_objects.header = objects_ptr->header; const auto & attention_lanelets = intersection_lanelets.attention(); const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); @@ -1422,10 +2356,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, false); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); if (belong_adjacent_lanelet_id) { continue; } @@ -1439,33 +2371,28 @@ util::TargetObjects IntersectionModule::generateTargetObjects( const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); if (belong_attention_lanelet_id) { const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; target_object.stopline = std::nullopt; target_objects.intersection_area_objects.push_back(target_object); } - } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, - planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); @@ -1485,10 +2412,10 @@ util::TargetObjects IntersectionModule::generateTargetObjects( } bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1496,14 +2423,8 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, lane_id_, associative_ids_, closest_idx, - last_intersection_stopline_candidate_idx, time_delay, - planner_param_.collision_detection.velocity_profile.default_velocity, - planner_param_.collision_detection.velocity_profile.minimum_default_velocity, - planner_param_.collision_detection.velocity_profile.use_upstream, - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity, - &ego_ttc_time_array); + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); if ( std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != @@ -1513,7 +2434,7 @@ bool IntersectionModule::checkCollision( } const double passing_time = time_distance_array.back().first; - util::cutPredictPathWithDuration(target_objects, clock_, passing_time); + cutPredictPathWithDuration(target_objects, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( @@ -1524,12 +2445,12 @@ bool IntersectionModule::checkCollision( // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); } - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); @@ -1538,7 +2459,7 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); - const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { if (!target_object.dist_to_stopline) { return false; } @@ -1553,7 +2474,7 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stopline > braking_distance; }; - const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { if ( !target_object.attention_lanelet || !target_object.dist_to_stopline || !target_object.stopline) { @@ -1612,14 +2533,14 @@ bool IntersectionModule::checkCollision( // If the vehicle is expected to stop before their stopline, ignore const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && !expected_to_stop_before_stopline && is_tolerable_overshoot) { debug_data_.red_overshoot_ignore_targets.objects.push_back(object); continue; @@ -1739,16 +2660,288 @@ bool IntersectionModule::checkCollision( return collision_detected; } +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + 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, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + return std::nullopt; +} + +void IntersectionModule::cutPredictPathWithDuration( + TargetObjects * target_objects, const double time_thr) +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = + rclcpp::Time(target_objects->header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); + } + } + } + } +} + +TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + + int assigned_lane_found = false; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + if (assigned_lane_found && !has_objective_lane_id) { + break; + } + assigned_lane_found = has_objective_lane_id; + } + if (!assigned_lane_found) { + return {{0.0, 0.0}}; // has already passed the intersection. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); + const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (has_upstream_stopline && i > upstream_stopline_ind) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +static 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 IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + 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); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr) + const TargetObjects & target_objects) { + const auto & occ_grid = *planner_data_->occupancy_grid; + const auto & current_pose = planner_data_->current_odometry->pose; + const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; + const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -2067,6 +3260,108 @@ IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( return OcclusionType::STATICALLY_OCCLUDED; } +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + +void IntersectionLanelets::update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + const auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } +} + +void TargetObject::calc_dist_to_stopline() +{ + if (!attention_lanelet || !stopline) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stopline_val = stopline.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; + stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; + stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); +} + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 4c33c0960afc3..38feada725872 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,290 @@ namespace behavior_velocity_planner using TimeDistanceArray = std::vector>; +struct DebugData +{ + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + 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 ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional first_attention_area{std::nullopt}; + std::optional second_attention_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + std::vector candidate_collision_object_polygons; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + 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}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_ lanelets + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the first conflicting lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the first attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * second_pass_judge_line is before second_attention_stopline by the braking distance. if its + * value is calculated negative, it is 0 + */ + size_t second_pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + std::optional dist_to_stopline{std::nullopt}; + void calc_dist_to_stopline(); +}; + +/** + * @struct + * @brief categorize TargetObjects + */ +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; + +/** + * @struct + * @brief categorize traffic light priority + */ +enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED +}; + class IntersectionModule : public SceneModuleInterface { public: @@ -74,8 +359,7 @@ class IntersectionModule : public SceneModuleInterface bool use_stuck_stopline; double stuck_vehicle_detect_dist; double stuck_vehicle_velocity_threshold; - double timeout_private_area; - bool enable_private_area_stuck_disregard; + bool disable_against_private_lane; } stuck_vehicle; struct YieldStuck @@ -160,33 +444,57 @@ class IntersectionModule : public SceneModuleInterface }; enum OcclusionType { + //! occlusion is not detected NOT_OCCLUDED, + //! occlusion is not caused by dynamic objects STATICALLY_OCCLUDED, + //! occlusion is caused by dynamic objects DYNAMICALLY_OCCLUDED, - RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + //! actual occlusion does not exist, only disapproved by RTC + RTC_OCCLUDED, }; + /** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ struct Indecisive { std::string error; }; + /** + * @struct + * @brief detected stuck vehicle + */ struct StuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; std::optional occlusion_stopline_idx{std::nullopt}; }; + /** + * @struct + * @brief yielded by vehicle on the attention area + */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; }; + /** + * @struct + * @brief only collision is detected + */ struct NonOccludedCollisionStop { size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ struct FirstWaitBeforeOcclusion { bool is_actually_occlusion_cleared{false}; @@ -194,23 +502,29 @@ class IntersectionModule : public SceneModuleInterface size_t first_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; - // A state peeking to occlusion limit line in the presence of traffic light + /** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ struct PeekingTowardOcclusion { - // NOTE: if intersection_occlusion is disapproved externally through RTC, - // it indicates "is_forcefully_occluded" + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" bool is_actually_occlusion_cleared{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck and shows up - // intersection_occlusion(x.y) + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck and shows up + //! intersection_occlusion(x.y) std::optional static_occlusion_timeout{std::nullopt}; }; - // A state detecting both collision and occlusion in the presence of traffic light + /** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -219,10 +533,14 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; }; + /** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ struct OccludedAbsenceTrafficLight { bool is_actually_occlusion_cleared{false}; @@ -232,13 +550,20 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; }; + /** + * @struct + * @brief both collision and occlusion are not detected + */ struct Safe { - // NOTE: if RTC is disapproved status, default stop lines are still needed. size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief traffic light is red or arrow signal + */ struct FullyPrioritized { bool collision_detected{false}; @@ -247,29 +572,25 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stopline_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - YieldStuckStop, // detected yield 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 - FullyPrioritized // only detect vehicles violating traffic rules + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield 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 + FullyPrioritized //! 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 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); + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); - /** - * @brief plan go-stop velocity at traffic crossing with collision check between reference path - * and object predicted path - */ bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; @@ -290,75 +611,169 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; + // Parameter + PlannerParam planner_param_; + + std::optional intersection_lanelets_{std::nullopt}; + + // for pass judge decision bool is_go_out_{false}; bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; - // Parameter - PlannerParam planner_param_; - - std::optional intersection_lanelets_{std::nullopt}; - // for occlusion detection const bool enable_occlusion_detection_; 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_; + std::nullopt}; //! for caching discretized occlusion detection lanelets + StateMachine collision_state_machine_; //! for stable collision checking + StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection StateMachine temporal_stop_before_attention_state_machine_; StateMachine static_occlusion_timeout_state_machine_; - // 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_; - // for RTC const UUID occlusion_uuid_; 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}; + /** + * @fn + * @brief set all RTC variable to safe and -inf + */ void initializeRTCStatus(); + /** + * @fn + * @brief analyze traffic_light/occupancy/objects context and return DecisionResult + */ + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief set RTC value according to calculated DecisionResult + */ void prepareRTCStatus( const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); + + /** + * @fn + * @brief generate IntersectionLanelets + */ + IntersectionLanelets getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); - bool checkStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets); + /** + * @fn + * @brief generate IntersectionStopLines + */ + std::optional generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - bool checkYieldStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area); + /** + * @fn + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( + const util::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet); + + /** + * @fn + * @brief generate PathLanelets + */ + std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx); - util::TargetObjects generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, + /** + * @fn + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief categorize target objects + */ + TargetObjects generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; + /** + * @fn + * @brief check collision + */ bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level); + + std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const; + void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); + + /** + * @fn + * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of + * (time of arrival, traveled distance) from current ego position + */ + TimeDistanceArray calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); + + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + + /** + * @fn + * @brief check occlusion status + */ OcclusionType getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr); + const TargetObjects & target_objects); + + /* + * @fn + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn(lanelet::ConstLanelet lane); /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -368,7 +783,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - util::DebugData debug_data_; + DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index b373f2cbc1c8a..b929959e43f12 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,17 +16,16 @@ #include "util.hpp" -#include #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -50,6 +49,32 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( state_machine_.setState(StateMachine::State::STOP); } +static std::optional getFirstConflictingLanelet( + const lanelet::ConstLanelets & conflicting_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & conflicting_lanelet : conflicting_lanelets) { + const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); + const bool intersects = bg::intersects(polygon_2d, path_footprint); + if (intersects) { + return std::make_optional(conflicting_lanelet); + } + } + } + return std::nullopt; +} + bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = DebugData(); @@ -83,44 +108,37 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return false; } - /* get detection area */ - if (!intersection_lanelets_) { - const auto & assigned_lanelet = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - *path, lanelet_map_ptr, planner_data_->current_odometry->pose); - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, - planner_param_.consider_wrong_direction_vehicle); - } - auto & intersection_lanelets = intersection_lanelets_.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update( - false, interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); - if (!first_conflicting_area) { + if (!first_conflicting_lanelet_) { + const auto conflicting_lanelets = getAttentionLanelets(); + first_conflicting_lanelet_ = getFirstConflictingLanelet( + conflicting_lanelets, interpolated_path_info, local_footprint, baselink2front); + } + if (!first_conflicting_lanelet_) { return false; } + const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - /* set stop-line and stop-judgement-line for base_link */ - const auto stopline_idx_opt = util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, interpolated_path_info, - planner_param_.stopline_margin, false, path); - if (!stopline_idx_opt.has_value()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, baselink2front); + if (!first_conflicting_idx_opt) { return false; } - - const size_t stopline_idx = stopline_idx_opt.value(); - if (stopline_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); + const auto stopline_idx_ip = static_cast(std::max( + 0, static_cast(first_conflicting_idx_opt.value()) - + static_cast(baselink2front / planner_param_.path_interpolation_ds))); + + const auto stopline_idx_opt = util::insertPointIndex( + interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + if (!stopline_idx_opt) { + RCLCPP_DEBUG(logger_, "failed to insert stopline, ignore planning."); return true; } + const auto stopline_idx = stopline_idx_opt.value(); - debug_data_.virtual_wall_pose = planning_utils::getAheadPose( - stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); debug_data_.stop_point_pose = path->points.at(stopline_idx).point.pose; /* set stop speed */ @@ -154,45 +172,33 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return true; } -autoware_auto_planning_msgs::msg::PathWithLaneId -MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length) +lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const { - if (path.points.size() < 2) { - return path; - } - - autoware_auto_planning_msgs::msg::PathWithLaneId private_path = path; - private_path.points.clear(); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - double sum_dist = 0.0; - bool prev_has_target_lane_id = false; - for (int i = static_cast(path.points.size()) - 2; i >= 0; i--) { - bool has_target_lane_id = false; - for (const auto path_id : path.points.at(i).lane_ids) { - if (path_id == lane_id_) { - has_target_lane_id = true; + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + lanelet::ConstLanelets sibling_lanelets; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + sibling_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(sibling_lanelets, following_lanelet)) { + continue; } + sibling_lanelets.push_back(following_lanelet); } - if (has_target_lane_id) { - // add path point with target lane id - // (lanelet with target lane id is exit of private road) - private_path.points.emplace_back(path.points.at(i)); - prev_has_target_lane_id = true; + } + + lanelet::ConstLanelets attention_lanelets; + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(sibling_lanelets, conflicting_lanelet)) { continue; } - if (prev_has_target_lane_id) { - // extend path to the front - private_path.points.emplace_back(path.points.at(i)); - sum_dist += tier4_autoware_utils::calcDistance2d( - path.points.at(i).point.pose, path.points.at(i + 1).point.pose); - if (sum_dist > extend_length) { - break; - } - } + attention_lanelets.push_back(conflicting_lanelet); } - - std::reverse(private_path.points.begin(), private_path.points.end()); - return private_path; + return attention_lanelets; } + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index fab0303640700..a44b99c97457d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,10 +15,7 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include "scene_intersection.hpp" - #include -#include #include #include @@ -27,9 +24,6 @@ #include #include -#include -#include - #include #include #include @@ -79,17 +73,15 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } + lanelet::ConstLanelets getAttentionLanelets() const; private: const int64_t lane_id_; const std::set associative_ids_; - autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); - // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional first_conflicting_lanelet_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 1c7e366347fec..6afc2d61d77f2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -18,48 +18,27 @@ #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 #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; @@ -83,7 +62,7 @@ static std::optional getDuplicatedPointIdx( return std::nullopt; } -static std::optional insertPointIndex( +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) @@ -155,66 +134,7 @@ std::optional> findLaneIdsInterval( return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; } -/** - * @brief Get stop point from map if exists - * @param stop_pose stop point defined on map - * @return true when the stop point is defined on map. - */ -static std::optional getStopLineIndexFromMap( - const InterpolatedPathInfo & interpolated_path_info, - const std::shared_ptr & planner_data) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - lanelet::ConstLanelet lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get( - interpolated_path_info.lane_id); - const auto road_markings = lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; i++) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(extended_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::optional getFirstPointInsidePolygonByFootprint( +std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { @@ -235,222 +155,6 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } - } - return std::nullopt; -} - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with detection_area - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // TODO(Mamoru Sobue): maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); - } - - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; - const bool first_attention_stopline_valid = true; - - // (4) pass judge line position on interpolated path - const double velocity = planner_data->current_velocity->twist.linear.x; - const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed - // first_conflicting_area, this could be null. - const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -489,105 +193,7 @@ std::optional getFirstPointInsidePolygon( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const auto lane_interval_ip_start = std::get<0>(lane_interval_ip); - size_t stuck_stopline_idx_ip = 0; - if (use_stuck_stopline) { - stuck_stopline_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stopline_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, conflicting_area); - if (!stuck_stopline_idx_ip_opt) { - return std::nullopt; - } - stuck_stopline_idx_ip = stuck_stopline_idx_ip_opt.value(); - } - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stopline_idx_ip) - 1 - stopline_margin_idx_dist - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - return insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - -/** - * @param pair lanelets and the vector of original lanelets in topological order (not reversed as - *in generateDetectionLaneDivisions()) - **/ -static std::pair> +std::pair> mergeLaneletsByTopologicalSort( const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr) @@ -682,207 +288,9 @@ mergeLaneletsByTopologicalSort( return {merged, originals}; } -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle) -{ - const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // If ego_lane has right of way (i.e. is high priority), - // ignore yieldLanelets (i.e. low priority lanes) - lanelet::ConstLanelets yield_lanelets{}; - const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); - for (const auto & right_of_way : right_of_ways) { - if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { - for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { - yield_lanelets.push_back(yield_lanelet); - for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { - yield_lanelets.push_back(previous_lanelet); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { - ego_lanelets.push_back(previous_lanelet); - for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { - if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { - continue; - } - ego_lanelets.push_back(following_lanelet); - } - } - - // get conflicting lanes on assigned lanelet - const auto & conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - std::vector adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const auto turn_direction = getTurnDirection(ll); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -892,8 +300,8 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -902,81 +310,13 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -/* -static std::vector getAllAdjacentLanelets( - const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) -{ - std::set results; - - results.insert(lane.id()); - - auto it = routing_graph->adjacentRight(lane); - // take all lane on the right side - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentRight(it.get()); - } - // take all lane on the left side - it = routing_graph->adjacentLeft(lane); - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentLeft(it.get()); - - } - return std::vector(results.begin(), results.end()); -} -*/ - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( - lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, - lanelet::ConstLanelet lane) -{ - // some of the intersections are not well-formed, and "adjacent" turning - // lanelets are not sharing the LineStrings - const std::string turn_direction = getTurnDirection(lane); - if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") - return {}; - - std::set previous_lanelet_ids; - for (auto && previous_lanelet : routing_graph->previous(lane)) { - previous_lanelet_ids.insert(previous_lanelet.id()); - } - - std::set besides_previous_lanelet_ids; - for (auto && previous_lanelet_id : previous_lanelet_ids) { - lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); - for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { - besides_previous_lanelet_ids.insert(beside_lanelet); - } - } - - std::set following_turning_lanelets; - following_turning_lanelets.insert(lane.id()); - for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { - lanelet::ConstLanelet besides_previous_lanelet = - map->laneletLayer.get(besides_previous_lanelet_id); - for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { - // if this has {"turn_direction", "${turn_direction}"}, take this - if (getTurnDirection(following_lanelet) == turn_direction) - following_turning_lanelets.insert(following_lanelet.id()); - } - } - lanelet::ConstLanelets ret{}; - for (auto && id : following_turning_lanelets) { - ret.push_back(map->laneletLayer.get(id)); - } - return ret; -} -*/ - -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); if (area_id_str == "else") return std::nullopt; - const int area_id = std::atoi(area_id_str.c_str()); + const lanelet::Id area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); Polygon2d poly{}; for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); @@ -993,111 +333,6 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - 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 TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - 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 TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_it->second; - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - 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, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds) -{ - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - 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); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -1115,7 +350,6 @@ std::optional generateInterpolatedPath( return interpolated_path_info; } -// from here geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) { @@ -1133,503 +367,5 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data) -{ - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, - const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) -{ - const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); - Polygon2d first_attention_area_poly; - for (const auto & p : first_attention_area_2d) { - first_attention_area_poly.outer().emplace_back(p.x(), p.y()); - } - - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; // not stop vehicle - } - - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - - // check if the object is too close to the ego path - if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { - continue; - } - - // check if the footprint is in the stuck detect area - const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); - if (is_in_stuck_area && debug_data) { - debug_data->yield_stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - Polygon2d polygon{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return polygon; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - if (target_polygon.empty()) { - return polygon; - } - - for (const auto & p : target_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - - polygon.outer().emplace_back(polygon.outer().front()); - bg::correct(polygon); - - return polygon; -} - -std::optional 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 dist_margin, const bool is_parked_vehicle) -{ - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - 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, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) -{ - const rclcpp::Time current_time = clock->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_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, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double current_velocity = planner_data->current_velocity->twist.linear.x; - - int assigned_lane_found = false; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - 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) { - break; - } - assigned_lane_found = has_objective_lane_id; - } - if (!assigned_lane_found) { - return {{0.0, 0.0}}; // has already passed the intersection. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { - return minimum_upstream_velocity; - } - return std::max(average_velocity, minimum_ego_velocity); - } else { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) -{ - const auto lane_id = assigned_lanelet.id(); - const auto intersection_first_itr = - std::find_if(path.points.cbegin(), path.points.cend(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if ( - intersection_first_itr == path.points.begin() || intersection_first_itr == path.points.end()) { - return 0.0; - } - const auto dst_idx = std::distance(path.points.begin(), intersection_first_itr) - 1; - - if (closest_idx > static_cast(dst_idx)) { - return 0.0; - } - - double distance = std::abs(motion_utils::calcSignedArcLength(path.points, closest_idx, dst_idx)); - const auto & lane_first_point = assigned_lanelet.centerline2d().front(); - distance += std::hypot( - path.points.at(dst_idx).point.pose.position.x - lane_first_point.x(), - path.points.at(dst_idx).point.pose.position.y - lane_first_point.y()); - return distance; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width) -{ - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; - } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); -} - } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 8d0e673fc931e..86a34d1e95114 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,12 +15,12 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "scene_intersection.hpp" #include "util_type.hpp" #include -#include +#include +#include #include #include @@ -35,88 +35,82 @@ namespace behavior_velocity_planner { namespace util { -std::optional insertPoint( + +/** + * @fn + * @brief insert a new pose to the path and return its index + * @return if insertion was successful return the inserted point index + */ +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); +/** + * @fn + * @brief check if a PathPointWithLaneId contains any of the given lane ids + */ bool hasLaneIds( const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); -std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief get objective polygons for detection area + * @fn + * @brief find the first contiguous interval of the path points that contains the specified lane ids + * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is + * found, returns the pair (start-1, end) */ -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle); +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief Generate a stop line for stuck vehicle - * @param conflicting_areas used to generate stop line for stuck vehicle - * @param original_path ego-car lane - * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car - * lane) - " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + * @fn + * @brief return the index of the first point which is inside the given polygon + * @param[in] lane_interval the interval of the path points on the intersection + * @param[in] search_forward flag for search direction */ -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); /** + * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose - * @param path path - * @param closest_idx ego's closest index on the path - * @param current_pose ego's exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); +/** + * @fn + * @brief check if ego is before the target_idx. If the index is same, compare the exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( -lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, -lanelet::ConstLanelet lane); -*/ + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); +/** + * @fn + * @brief check if the given lane has related traffic light + */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds); - +/** + * @fn + * @brief interpolate PathWithLaneId + */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -125,55 +119,28 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data); - -bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, - const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data); - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); - -std::optional 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 dist_margin, const bool is_parked_vehicle); - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, - const double time_thr); +/** + * @fn + * @brief this function sorts the set of lanelets topologically using topological sort and merges + * the lanelets from each root to each end. each branch is merged and returned with the original + * lanelets + * @param[in] lanelets the set of lanelets + * @param[in] routing_graph_ptr the routing graph + * @return the pair of merged lanelets and their corresponding original lanelets + */ +std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr); -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_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, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); - -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval); - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width); +/** + * @fn + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygon + */ +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 3c7ba3041b0bd..763d829dacf9b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -34,177 +34,24 @@ namespace behavior_velocity_planner::util { -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - 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 ego_lane{std::nullopt}; - std::optional> adjacent_area{std::nullopt}; - std::optional stuck_vehicle_detect_area{std::nullopt}; - std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - 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}; - std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; -}; - +/** + * @struct + * @brief wrapper class of interpolated path with lane id + */ struct InterpolatedPathInfo { + /** the interpolated path */ autoware_auto_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ double ds{0.0}; + /** the intersection lanelet id */ lanelet::Id lane_id{0}; + /** the associative lane ids of lane_id */ std::set associative_lane_ids{}; + /** the range of indices for the path points with associative lane id */ std::optional> lane_id_interval{std::nullopt}; }; -struct IntersectionLanelets -{ -public: - void update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - - lanelet::ConstLanelets attention_; // topologically merged lanelets - std::vector> - attention_stoplines_; // the stop lines for each attention_ lanelets - lanelet::ConstLanelets attention_non_preceding_; - std::vector> - attention_non_preceding_stoplines_; // the stop lines for each attention_non_preceding_ - // lanelets - lanelet::ConstLanelets conflicting_; - lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets - std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets - std::vector attention_area_; // topologically merged lanelets - std::vector attention_non_preceding_area_; - std::vector conflicting_area_; - std::vector adjacent_area_; - std::vector - occlusion_attention_area_; // topologically merged lanelets - // the first area intersecting with the path - // even if lane change/re-routing happened on the intersection, these areas area are supposed to - // be invariant under the 'associative' lanes. - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - bool is_prioritized_ = false; -}; - -struct IntersectionStopLines -{ - // NOTE: for baselink - size_t closest_idx{0}; - // NOTE: null if path does not conflict with first_conflicting_area - std::optional stuck_stopline{std::nullopt}; - // NOTE: null if path is over map stopline OR its value is calculated negative - std::optional default_stopline{std::nullopt}; - // NOTE: null if the index is calculated negative - std::optional first_attention_stopline{std::nullopt}; - // NOTE: null if footprints do not change from outside to inside of detection area - std::optional occlusion_peeking_stopline{std::nullopt}; - // if the value is calculated negative, its value is 0 - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - -enum class TrafficPrioritizedLevel { - // The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - // The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - // The target lane's traffic signal is green - NOT_PRIORITIZED -}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 47d4985ae643e..e049d02ffe9b5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -230,6 +230,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( } const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); if (obj_v > planner_param_.stuck_vehicle_vel_thr) { + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::GREEN); continue; // not stop vehicle } // check if the footprint is in the stuck detect area @@ -237,6 +239,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); for (const auto & p : obj_footprint.outer()) { geometry_msgs::msg::Point point; point.x = p.x(); diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index aa51c38b55742..b31506918a2db 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: forward_path_length: 1000.0 backward_path_length: 5.0 + behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 max_accel: -2.8 max_jerk: -5.0 diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 22220ccd182a1..c1631fdb739de 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -26,6 +26,7 @@ + @@ -69,6 +70,7 @@ + diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 7c0036b7812fa..e2c29ef868257 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -138,6 +138,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Parameters forward_path_length_ = declare_parameter("forward_path_length"); backward_path_length_ = declare_parameter("backward_path_length"); + behavior_output_path_interval_ = declare_parameter("behavior_output_path_interval"); planner_data_.stop_line_extend_length = declare_parameter("stop_line_extend_length"); // nearest search @@ -147,6 +148,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } planner_manager_.launchScenePlugin(*this, name); } @@ -322,7 +327,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + if ( + is_unknown_observation && + planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } } } @@ -405,7 +424,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + const auto interpolated_path_msg = + interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index ded1d08700a1d..a345f1200f721 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -102,6 +102,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // parameter double forward_path_length_; double backward_path_length_; + double behavior_output_path_interval_; // member PlannerData planner_data_; diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 491df4c7a8766..239abbde27609 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2ea9f6ed2648b..606e41ad4b1d1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -76,7 +76,10 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -125,12 +128,20 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal(const int id) const + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; if (traffic_light_id_map.count(id) == 0) { - return {}; + return std::nullopt; } - return std::make_shared(traffic_light_id_map.at(id)); + return std::make_optional(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index ced2e267cc025..fcde16d8a871c 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,8 @@ namespace behavior_velocity_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; +using objects_of_interest_marker_interface::ColorName; +using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; using tier4_debug_msgs::msg::Float64Stamped; @@ -55,6 +58,19 @@ using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; using unique_identifier_msgs::msg::UUID; +struct ObjectOfInterest +{ + geometry_msgs::msg::Pose pose; + autoware_auto_perception_msgs::msg::Shape shape; + ColorName color; + ObjectOfInterest( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + : pose(pose), shape(shape), color(color_name) + { + } +}; + class SceneModuleInterface { public: @@ -94,6 +110,8 @@ class SceneModuleInterface void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } + std::vector getObjectsOfInterestData() const { return objects_of_interest_; } + void clearObjectsOfInterestData() { objects_of_interest_.clear(); } protected: const int64_t module_id_; @@ -107,6 +125,7 @@ class SceneModuleInterface std::optional infrastructure_command_; std::optional first_stop_path_point_index_; VelocityFactorInterface velocity_factor_; + std::vector objects_of_interest_; void setSafe(const bool safe) { @@ -118,6 +137,13 @@ class SceneModuleInterface void setDistance(const double distance) { distance_ = distance; } void syncActivation() { setActivation(isSafe()); } + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + { + objects_of_interest_.emplace_back(pose, shape, color_name); + } + size_t findEgoSegmentIndex( const std::vector & points) const; }; @@ -200,6 +226,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface RTCInterface rtc_interface_; std::unordered_map map_uuid_; + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + virtual void sendRTC(const Time & stamp); virtual void setActivation(); @@ -220,6 +248,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void removeUUID(const int64_t & module_id); + void publishObjectsOfInterestMarker(); + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 591ef8667b81b..667c915ac1d7f 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -28,8 +28,7 @@ bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, - const double interval = 1.0); + const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( const autoware_auto_planning_msgs::msg::Path & path); autoware_auto_planning_msgs::msg::Path filterStopPathPoint( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index c86272cff1c2e..2184be4fbd2fe 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path); -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory); - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 187720ff92a15..784b7cabfe9ad 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -32,6 +32,7 @@ motion_utils motion_velocity_smoother nav_msgs + objects_of_interest_marker_interface pcl_conversions rclcpp rclcpp_components diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 49a52fcd60df2..362493005eb17 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -174,7 +174,7 @@ void SceneModuleManagerInterface::deleteExpiredModules( void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); scene_modules_.insert(scene_module); @@ -183,7 +183,7 @@ void SceneModuleManagerInterface::registerModule( void SceneModuleManagerInterface::unregisterModule( const std::shared_ptr & scene_module) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "unregister task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.erase(scene_module->getModuleId()); @@ -192,7 +192,9 @@ void SceneModuleManagerInterface::unregisterModule( SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) { } @@ -202,6 +204,7 @@ void SceneModuleManagerInterfaceWithRTC::plan( setActivation(); modifyPathVelocity(path); sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); } void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) @@ -244,6 +247,19 @@ void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) } } +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 7fe8612cc6858..314281a954b49 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. // #include +#include "motion_utils/trajectory/conversion.hpp" + #include #include #include @@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) -{ - TrajectoryPoints tps; - for (const auto & p : path.points) { - TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - tps.emplace_back(tp); - } - return tps; -} -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) -{ - PathWithLaneId path; - for (const auto & p : trajectory) { - PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - path.points.emplace_back(pp); - } - return path; -} - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, @@ -87,7 +54,9 @@ bool smoothPath( const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - auto trajectory = convertPathToTrajectoryPoints(in_path); + auto trajectory = + motion_utils::convertToTrajectoryPoints( + in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = @@ -117,7 +86,7 @@ bool smoothPath( motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } - out_path = convertTrajectoryPointsToPath(traj_smoothed); + out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index ea63462b32d84..5534228c1b86f 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,6 +3,7 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet: specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index 7bd27fca3407c..809579b383461 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -20,6 +20,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + behavior_velocity_crosswalk_module behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 91656d542ea8e..b28725a92628e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -35,14 +35,17 @@ class DebugValues public: enum class TYPE { CALCULATION_TIME = 0, - CALCULATION_TIME_COLLISION_CHECK = 1, - LATERAL_DIST = 2, - LONGITUDINAL_DIST_OBSTACLE = 3, - LONGITUDINAL_DIST_COLLISION = 4, - COLLISION_POS_FROM_EGO_FRONT = 5, - STOP_DISTANCE = 6, - NUM_OBSTACLES = 7, - LATERAL_PASS_DIST = 8, + CALCULATION_TIME_PATH_PROCESSING = 1, + CALCULATION_TIME_OBSTACLE_CREATION = 2, + CALCULATION_TIME_COLLISION_CHECK = 3, + CALCULATION_TIME_PATH_PLANNING = 4, + LATERAL_DIST = 5, + LONGITUDINAL_DIST_OBSTACLE = 6, + LONGITUDINAL_DIST_COLLISION = 7, + COLLISION_POS_FROM_EGO_FRONT = 8, + STOP_DISTANCE = 9, + NUM_OBSTACLES = 10, + LATERAL_PASS_DIST = 11, SIZE, // this is the number of enum elements }; diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 09c87ed81cf38..3ba9bf8bf52e6 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) auto & p = planner_param_.run_out; p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk"); p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin"); diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 2fa8cf241ee74..b14152863d8d0 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -14,6 +14,7 @@ #include "scene.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" #include @@ -23,6 +24,10 @@ #include #include +#include + +#include +#include #include #include @@ -60,7 +65,7 @@ bool RunOutModule::modifyPathVelocity( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { // timer starts - const auto t1_modify_path = std::chrono::system_clock::now(); + const auto t_start = std::chrono::system_clock::now(); // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; @@ -70,20 +75,27 @@ bool RunOutModule::modifyPathVelocity( // set height of debug data debug_ptr_->setHeight(current_pose.position.z); - // smooth velocity of the path to calculate time to collision accurately - PathWithLaneId smoothed_path; - if (!smoothPath(*path, smoothed_path, planner_data_)) { - return true; - } - // extend path to consider obstacles after the goal - const auto extended_smoothed_path = - run_out_utils::extendPath(smoothed_path, planner_param_.vehicle_param.base_to_front); + const auto extended_path = + run_out_utils::extendPath(*path, planner_param_.vehicle_param.base_to_front); // trim path ahead of the base_link to make calculation easier const double trim_distance = planner_param_.run_out.detection_distance; - const auto trim_smoothed_path = - run_out_utils::trimPathFromSelfPose(extended_smoothed_path, current_pose, trim_distance); + const auto trim_path = + run_out_utils::trimPathFromSelfPose(extended_path, current_pose, trim_distance); + + // smooth velocity of the path to calculate time to collision accurately + PathWithLaneId extended_smoothed_path; + if (!smoothPath(trim_path, extended_smoothed_path, planner_data_)) { + return true; + } + + // record time for path processing + const auto t_path_processing = std::chrono::system_clock::now(); + const auto elapsed_path_processing = + std::chrono::duration_cast(t_path_processing - t_start); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_PATH_PROCESSING, elapsed_path_processing.count() / 1000.0); // create abstracted dynamic obstacles from objects or points dynamic_obstacle_creator_->setData(*planner_data_, planner_param_, *path, extended_smoothed_path); @@ -92,18 +104,30 @@ bool RunOutModule::modifyPathVelocity( // extract obstacles using lanelet information const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, trim_smoothed_path, current_pose); + excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); - // timer starts - const auto t1_collision_check = std::chrono::system_clock::now(); + // record time for obstacle creation + const auto t_obstacle_creation = std::chrono::system_clock::now(); + const auto elapsed_obstacle_creation = + std::chrono::duration_cast(t_obstacle_creation - t_path_processing); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_OBSTACLE_CREATION, + elapsed_obstacle_creation.count() / 1000.0); // detect collision with dynamic obstacles using velocity planning of ego - const auto dynamic_obstacle = detectCollision(partition_excluded_obstacles, trim_smoothed_path); - - // timer ends - const auto t2_collision_check = std::chrono::system_clock::now(); + const auto crosswalk_lanelets = planner_param_.run_out.suppress_on_crosswalk + ? getCrosswalksOnPath( + planner_data_->current_odometry->pose, *path, + planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->route_handler_->getOverallGraphPtr()) + : std::vector>(); + const auto dynamic_obstacle = + detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets); + + // record time for collision check + const auto t_collision_check = std::chrono::system_clock::now(); const auto elapsed_collision_check = - std::chrono::duration_cast(t2_collision_check - t1_collision_check); + std::chrono::duration_cast(t_collision_check - t_obstacle_creation); debug_ptr_->setDebugValues( DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); @@ -112,7 +136,7 @@ bool RunOutModule::modifyPathVelocity( // after a certain amount of time has elapsed since the ego stopped, // approach the obstacle with slow velocity insertVelocityForState( - dynamic_obstacle, *planner_data_, planner_param_, trim_smoothed_path, *path); + dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path); } else { // just insert zero velocity for stopping insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); @@ -124,20 +148,26 @@ bool RunOutModule::modifyPathVelocity( } publishDebugValue( - trim_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); - // timer ends - const auto t2_modify_path = std::chrono::system_clock::now(); - const auto elapsed_modify_path = - std::chrono::duration_cast(t2_modify_path - t1_modify_path); + // record time for collision check + const auto t_path_planning = std::chrono::system_clock::now(); + const auto elapsed_path_planning = + std::chrono::duration_cast(t_path_planning - t_collision_check); debug_ptr_->setDebugValues( - DebugValues::TYPE::CALCULATION_TIME, elapsed_modify_path.count() / 1000.0); + DebugValues::TYPE::CALCULATION_TIME_PATH_PLANNING, elapsed_path_planning.count() / 1000.0); + + // record time for the function + const auto t_end = std::chrono::system_clock::now(); + const auto elapsed_all = std::chrono::duration_cast(t_end - t_start); + debug_ptr_->setDebugValues(DebugValues::TYPE::CALCULATION_TIME, elapsed_all.count() / 1000.0); return true; } std::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const std::vector> & crosswalk_lanelets) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -171,7 +201,7 @@ std::optional RunOutModule::detectCollision( debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0); auto obstacles_collision = - checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time); + checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets); if (obstacles_collision.empty()) { continue; } @@ -291,7 +321,8 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time) const + std::vector poly, const float travel_time, + const std::vector> & crosswalk_lanelets) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -320,8 +351,8 @@ std::vector RunOutModule::checkCollisionWithObstacles( *predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel}; std::vector collision_points; - const bool collision_detected = - checkCollisionWithShape(bg_poly_vehicle, pose_with_range, obstacle.shape, collision_points); + const bool collision_detected = checkCollisionWithShape( + bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points); if (!collision_detected) { continue; @@ -380,6 +411,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( bool RunOutModule::checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + const std::vector> & crosswalk_lanelets, std::vector & collision_points) const { bool collision_detected = false; @@ -402,6 +434,23 @@ bool RunOutModule::checkCollisionWithShape( break; } + if (!collision_points.empty()) { + for (const auto & crosswalk : crosswalk_lanelets) { + const auto poly = crosswalk.second.polygon2d().basicPolygon(); + for (auto it = collision_points.begin(); it != collision_points.end();) { + if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) { + it = collision_points.erase(it); + } else { + ++it; + } + } + if (collision_points.empty()) { + break; + } + } + collision_detected = !collision_points.empty(); + } + return collision_detected; } @@ -566,16 +615,25 @@ std::optional RunOutModule::calcStopPoint( // vehicle have to decelerate if there is not enough distance with deceleration_jerk const bool deceleration_needed = *stop_dist > dist_to_collision - planner_param_.run_out.stop_margin; - // avoid acceleration when ego is decelerating - // TODO(Tomohito Ando): replace with more appropriate method - constexpr float epsilon = 1.0e-2; - constexpr float stopping_vel_mps = 2.5 / 3.6; - const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; - if (!deceleration_needed && !is_stopping) { + const auto & detection_method = planner_param_.run_out.detection_method; + + if (!deceleration_needed && detection_method == "Object") { debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); return {}; } + // If the detection method assumes running out, avoid acceleration when the ego is decelerating. + // TODO(Tomohito Ando): replace with more appropriate way + if (!deceleration_needed && detection_method != "Object") { + constexpr float epsilon = 1.0e-2; + constexpr float stopping_vel_mps = 2.5 / 3.6; + const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; + if (!is_stopping) { + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); + return {}; + } + } + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index b1c49189267d4..def90f036c440 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -67,7 +68,8 @@ class RunOutModule : public SceneModuleInterface Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; std::optional detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const std::vector> & crosswalk_lanelets); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -77,7 +79,8 @@ class RunOutModule : public SceneModuleInterface std::vector checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time) const; + std::vector poly, const float travel_time, + const std::vector> & crosswalk_lanelets) const; std::optional findNearestCollisionObstacle( const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, @@ -89,6 +92,7 @@ class RunOutModule : public SceneModuleInterface bool checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + const std::vector> & crosswalk_lanelets, std::vector & collision_points) const; bool checkCollisionWithCylinder( diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 6afe451f72f73..5524c0f76049d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -54,6 +54,7 @@ struct RunOutParam { std::string detection_method; bool use_partition_lanelet; + bool suppress_on_crosswalk; bool specify_decel_jerk; double stop_margin; double passing_margin; diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index c658f0890b986..33bb471e5920c 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -24,7 +24,6 @@ eigen geometry_msgs lanelet2_extension - libboost-dev motion_utils pluginlib rclcpp @@ -34,6 +33,7 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs + traffic_light_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 9a0ba4f37c3c0..b3dedaa29d6ad 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -26,8 +26,6 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; - visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 908627540bdcc..fa1a516c107b0 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index c36c6af1128ce..87213d8a5ed3f 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -26,6 +26,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -57,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; - boost::optional first_ref_stop_path_point_index_; + std::optional first_ref_stop_path_point_index_; }; class TrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a74ff8c0cb5e8..9982ec34c4bca 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -16,10 +16,10 @@ #include #include +#include #include #include -#include // To be replaced by std::optional in C++17 #include @@ -31,9 +31,7 @@ #include #include -#include #include -#include #include namespace behavior_velocity_planner @@ -52,14 +50,14 @@ bool getBackwardPointFromBasePoint( return true; } -boost::optional findNearestCollisionPoint( +std::optional findNearestCollisionPoint( const LineString2d & line1, const LineString2d & line2, const Point2d & origin) { std::vector collision_points; bg::intersection(line1, line2, collision_points); if (collision_points.empty()) { - return boost::none; + return std::nullopt; } // check nearest collision point @@ -220,7 +218,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Move to go out state if ego vehicle over deadline. constexpr double signed_deadline_length = -2.0; if (signed_arc_length_to_stop_point < signed_deadline_length) { - RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); + RCLCPP_DEBUG(logger_, "APPROACH -> GO_OUT"); state_ = State::GO_OUT; stop_signal_received_time_ptr_.reset(); return true; @@ -266,7 +264,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * constexpr double restart_length = 1.0; if (use_initialization_after_start) { if (signed_arc_length_to_stop_point > restart_length) { - RCLCPP_INFO(logger_, "GO_OUT(RESTART) -> APPROACH"); + RCLCPP_DEBUG(logger_, "GO_OUT(RESTART) -> APPROACH"); state_ = State::APPROACH; } } @@ -290,7 +288,7 @@ bool TrafficLightModule::isStopSignal() return true; } - return isTrafficSignalStop(looking_tl_state_); + return traffic_light_utils::isTrafficSignalStop(lane_, looking_tl_state_); } void TrafficLightModule::updateTrafficSignal() @@ -352,48 +350,18 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const } } -bool TrafficLightModule::isTrafficSignalStop( - const autoware_perception_msgs::msg::TrafficSignal & tl_state) const -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - const std::string turn_direction = lane_.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); - if (!traffic_signal_stamped) { + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( + traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + if (!traffic_signal_stamped_opt) { RCLCPP_WARN_THROTTLE( logger_, *clock_, 5000 /* ms */, "the traffic signal data associated with regulatory element id is not received"); return false; } - - valid_traffic_signal = *traffic_signal_stamped; + valid_traffic_signal = traffic_signal_stamped_opt.value(); return true; } @@ -455,25 +423,4 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP return modified_path; } -bool TrafficLightModule::hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool TrafficLightModule::hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 6e46709267427..102ddbe2e9fa8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -81,7 +80,7 @@ class TrafficLightModule : public SceneModuleInterface inline State getTrafficLightModuleState() const { return state_; } - inline boost::optional getFirstRefStopPathPointIndex() const + inline std::optional getFirstRefStopPathPointIndex() const { return first_ref_stop_path_point_index_; } @@ -89,8 +88,6 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - bool isTrafficSignalStop(const TrafficSignal & tl_state) const; - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, @@ -98,10 +95,6 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const; - - bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const; bool isTrafficSignalTimedOut() const; @@ -130,9 +123,9 @@ class TrafficLightModule : public SceneModuleInterface // prevent stop chattering std::unique_ptr