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/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 99% 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..da7121bf4e9b5 100644 --- a/common/autoware_auto_geometry/include/geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp @@ -16,10 +16,10 @@ /// \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 @@ -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..e566e2e43ae3e 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 "autoware_auto_geometry/common_2d.hpp" #include -#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 95% 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..fd04c0266e065 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 "autoware_auto_geometry/common_2d.hpp" #include -#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 98% rename from common/autoware_auto_geometry/include/geometry/interval.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp index 59c26f27cc454..17e2789ac0325 100644 --- a/common/autoware_auto_geometry/include/geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp @@ -18,8 +18,8 @@ // 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" @@ -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..c689638aa2bd9 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,11 @@ /// \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 "autoware_auto_geometry/interval.hpp" #include "common/types.hpp" -#include "geometry/interval.hpp" #include #include @@ -175,4 +175,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 98% 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..c160685fb0f8c 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 "autoware_auto_geometry/spatial_hash_config.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" #include -#include -#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..926b170f807c2 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 "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" #include "helper_functions/crtp.hpp" #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..092f8a04f2540 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 "autoware_auto_geometry/lookup_table.hpp" + #include -#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..1fdc44df11183 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 @@ -120,7 +120,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( 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..683877f6429bd 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 @@ -91,6 +91,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; @@ -254,10 +255,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; } 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/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 067173288e685..2ba80fddc1f34 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 @@ -73,7 +73,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..d10bc8ef5ab9d 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,7 +237,7 @@ 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.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color = color_rgba; return marker_ptr; } @@ -267,7 +267,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; @@ -299,7 +299,7 @@ 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; 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..5aca4290e5ac3 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 @@ -154,7 +154,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..603e39f0c46b5 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,15 @@ 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); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -113,7 +134,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/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/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp similarity index 92% rename from common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp rename to common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 28d783aa4d293..804ae47ff0f4e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" @@ -45,4 +45,4 @@ std::vector convertToTrajecto } // namespace motion_utils -#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ +#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp similarity index 97% rename from common/motion_utils/src/trajectory/tmp_conversion.cpp rename to common/motion_utils/src/trajectory/conversion.cpp index 6f4a4039832bd..97a0bcd06e8cc 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 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 6664e6154bdfd..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" 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/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/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/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..5457a6c310f5d 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 @@ -48,14 +48,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 +64,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..6a5c61041f0da 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 @@ -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 @@ -174,6 +177,8 @@ class NDTScanMatcher : public rclcpp::Node tf2_ros::TransformBroadcaster tf2_broadcaster_; + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; + std::shared_ptr ndt_ptr_; std::shared_ptr> state_ptr_; @@ -191,8 +196,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,11 +203,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::atomic is_activated_; std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr map_update_module_; 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..655aa8e17ccd9 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -7,7 +7,6 @@ - @@ -27,7 +26,6 @@ - diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index f0a583417fb76..446f926a1d3f7 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_; } @@ -145,21 +92,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 +123,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..7f6fe605c42b0 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,17 @@ 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), 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 +163,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,21 +255,20 @@ 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); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); tf2_listener_module_ = std::make_shared(this); 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); + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, sensor_callback_group); } logger_configure_ = std::make_unique(this); @@ -368,6 +342,29 @@ void NDTScanMatcher::publish_diagnostic() diagnostics_pub_->publish(diag_msg); } +void NDTScanMatcher::callback_timer() +{ + if (!is_activated_) { + return; + } + if (!use_dynamic_map_loading_) { + 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 +379,11 @@ 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."); } + + if (use_dynamic_map_loading_) { + 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 +475,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 +561,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(); @@ -731,6 +732,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) 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..02ca62a61717c 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,114 @@ 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 [[maybe_unused]], const double delta_time) + { + 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(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..0091fc6bc38cb 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -740,6 +740,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 +795,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 +997,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; } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); + + // 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; + } + + // 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; 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_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/planning/.pages b/planning/.pages index 6ae8abe240d9b..c41a1cc8c6603 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 + - 'Avoidance': planning/behavior_path_avoidance_module + - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - '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 + - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - - '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 + - '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_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/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index e245f1c44fe0e..66d11d044966e 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 @@ -54,6 +54,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto & data = avoidance_data_; if (data.target_objects.empty()) { + RCLCPP_DEBUG(logger_, "no empty objects"); return false; } @@ -73,11 +74,13 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object); if (num_of_avoidance_targets < 1) { + RCLCPP_DEBUG(logger_, "no avoidance target"); return false; } const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "no empty lanes"); return false; } 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 3041ae7e1112e..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 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_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 be8bcb3fb970d..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 @@ -638,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/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index eb5cb18ce697c..b9af50ce76eb5 100644 --- 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 @@ -207,6 +207,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) 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 = 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 91c115706e030..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 @@ -325,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. */ 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 9ce331e70d8b2..4508cbc3c18f6 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 @@ -153,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, @@ -173,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/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index ee36f8c33149d..ee97efe37d440 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -618,6 +618,8 @@ MarkerArray createDebugMarkerArray( 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/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index a05bcd90bdfb9..31849c716971c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -17,7 +17,6 @@ #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" @@ -738,7 +737,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); @@ -874,7 +873,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(); } @@ -887,9 +886,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, @@ -1208,6 +1211,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; } @@ -1279,118 +1288,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; 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 01bfc375e9515..8ed56ce9aff4d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -890,7 +890,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { continue; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6abfc2e107348..493ca35f09a3b 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -16,6 +16,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.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/traffic_light_utils.hpp" @@ -225,6 +226,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 @@ -635,18 +728,7 @@ bool isSatisfiedWithVehicleCondition( return true; } - // always ignore all merging objects. - if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; - return false; - } - - // 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; - } - + // 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()); @@ -659,17 +741,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; } - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { - return true; + 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::NONE) { - object.reason = "ParallelToEgoLane"; + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; return false; } @@ -1654,12 +1745,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()); } @@ -1675,7 +1766,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 = @@ -1733,6 +1825,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 @@ -1752,6 +1847,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 @@ -1771,6 +1869,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; @@ -2047,4 +2148,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 7b9a608aecec3..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,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_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 index ff5be388704da..434988cc3ab08 100644 --- 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 @@ -14,8 +14,8 @@ #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 "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include 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_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index a1fa2068b9ab5..bffa5201f2882 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -155,7 +155,7 @@ stop #### Candidate Path's Safety check -See [safety check utils explanation](../behavior_path_planner/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 @@ -163,7 +163,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![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](../behavior_path_planner/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 diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index b9b44d3c96186..4e741a2b3db2c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -67,13 +67,15 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - bool isRootLaneletToBeUpdated() const override { return current_state_ == ModuleStatus::SUCCESS; } - - // 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; @@ -118,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; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 50766621b2f44..9c3c371627a78 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -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, diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index aa3ee0dc4c98b..451899fbf27e6 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.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; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 00793532023b9..22d85a2a1fc2a 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -64,7 +64,7 @@ void LaneChangeInterface::processOnExit() bool LaneChangeInterface::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } @@ -76,110 +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(), *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(), *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( @@ -188,6 +84,11 @@ void LaneChangeInterface::updateData() module_type_->resetStopPose(); } +void LaneChangeInterface::postProcess() +{ + post_process_safety_status_ = module_type_->isApprovedPathSafe(); +} + BehaviorModuleOutput LaneChangeInterface::plan() { resetPathCandidate(); @@ -286,6 +187,130 @@ 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; + } + + 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(); @@ -334,7 +359,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; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f0972d66416c8..c7ddd8fe63636 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1526,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_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 index 7a606b5d126ce..82f721411d5a4 100644 --- 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 @@ -14,8 +14,8 @@ #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 "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 40a23a09030de..6f462e17248f0 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 | +| 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 @@ -168,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 @@ -185,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 @@ -208,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 @@ -228,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/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 854c29aa89bc5..29fdca78e5bcb 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -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: 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/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/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 36106f32158b9..3ceff4244f2c8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -87,6 +87,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{}; @@ -106,6 +107,12 @@ struct DynamicAvoidanceParameters double end_duration_to_avoid_oncoming_object{0.0}; }; +struct TimeWhileCollision +{ + double time_to_start_collision; + double time_to_end_collision; +}; + class DynamicAvoidanceModule : public SceneModuleInterface { public: @@ -324,9 +331,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface 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( @@ -334,12 +341,13 @@ class DynamicAvoidanceModule : public SceneModuleInterface const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; 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 & 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 & ref_path_points_for_obj_poly, const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, 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 85abf774d159e..9fd397bb8a8b1 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -73,11 +73,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); @@ -264,7 +259,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"); @@ -447,12 +441,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(); @@ -929,8 +917,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/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 8772a1543b20b..87a940d04e8d3 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -570,25 +570,30 @@ void DynamicAvoidanceModule::updateTargetObjects() // } // 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)) { + 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; + } } // 2.f. calculate which side object will be against ego's path @@ -630,7 +635,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, - time_to_collision); + time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, prev_object); @@ -716,26 +721,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( @@ -928,7 +967,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( 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(ref_path_points_for_obj_poly, obj_pose.position); @@ -943,40 +982,54 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( 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); + }(); - if (obj_vel < 0) { - return MinMaxValue{ - raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; - } + const double relative_velocity = getEgoSpeed() - obj_vel; - // 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; + // 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; + } + // 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); - - const double valid_length_to_avoid = calcValidLengthToAvoid(obj_path, obj_pose, obj_shape); - if (obj_vel < -0.5) { + // 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_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 (0.5 < obj_vel) { + 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, - std::min(obj_lon_offset.max_value + end_length_to_avoid, valid_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, @@ -985,25 +1038,44 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( 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; } @@ -1015,6 +1087,43 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( } return obj_path.path.size() - 1; }(); + + // 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); } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index ca0bc070d0fdb..73d263319c2d9 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -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 @@ -207,6 +210,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 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 f2c22f774fc77..28ee6d31e80dc 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 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/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index c27c513832fa8..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. */ @@ -362,8 +362,84 @@ 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_; + ModuleStatus current_state_{ModuleStatus::IDLE}; + BehaviorModuleOutput previous_module_output_; StopReason stop_reason_; @@ -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. @@ -594,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/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index cfaa8de9b2fb9..1ce3dd3736276 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 @@ -81,8 +81,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 acda01dfc603c..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 @@ -76,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/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/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 e60b35b95515f..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 @@ -26,7 +26,6 @@ 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); 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 a7b91e5f7a8c9..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; @@ -45,16 +48,27 @@ using SegmentRtree = boost::geometry::index::rtree 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_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index ec182ae5b909a..9e20b9c3f8714 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -236,90 +236,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 (const auto & left_l : left_ls) { - marker.points.push_back(createPoint(left_l.x(), left_l.y(), left_l.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) 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 866d114815a86..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 @@ -120,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( @@ -184,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()); @@ -207,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) @@ -227,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(); @@ -270,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, @@ -280,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( @@ -303,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/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index ef676c8aaeb8b..ec147a7bb2771 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 @@ -33,23 +33,33 @@ namespace { template size_t findNearestSegmentIndexFromLateralDistance( - const std::vector & points, const geometry_msgs::msg::Point & target_point) + 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); - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + 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 tier4_autoware_utils::calcDistance2d(points.at(seg_idx), target_point); + return calcDistance2d(points.at(seg_idx), target_point.position); } 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.position); } - return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -61,7 +71,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return motion_utils::findNearestSegmentIndex(points, target_point); + return motion_utils::findNearestSegmentIndex(points, target_point.position); } bool checkHasSameLane( @@ -795,17 +805,17 @@ 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( right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); + motion_utils::findNearestSegmentIndex(left_bound, left_start_point); const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); + motion_utils::findNearestSegmentIndex(right_bound, right_start_point); // Insert a start point path.left_bound.push_back(left_start_point); @@ -817,18 +827,17 @@ 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 = calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); + goal_left_start_idx, motion_utils::findNearestSegmentIndex(left_bound, left_goal_point)); const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); + goal_right_start_idx, motion_utils::findNearestSegmentIndex(right_bound, right_goal_point)); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { @@ -1008,34 +1017,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) @@ -1085,13 +1066,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(), 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/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/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 96% 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..1a730212f828b 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -8,11 +8,11 @@ This module is activated when a new route is received. Use cases are as follows - start smoothly from the current ego position to centerline. - ![case1](../image/start_from_road_lane.drawio.svg) + ![case1](./images/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - ![case2](../image/start_from_road_side.drawio.svg) + ![case2](./images/start_from_road_side.drawio.svg) - pull out from the shoulder lane to the road lane centerline. - ![case3](../image/start_from_road_shoulder.drawio.svg) + ![case3](./images/start_from_road_shoulder.drawio.svg) ## Design @@ -78,14 +78,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 +103,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 +198,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 +220,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,7 +239,7 @@ 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) @@ -257,7 +257,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 +276,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_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_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_lane.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_from_road_lane.drawio.svg rename to planning/behavior_path_start_planner_module/images/start_from_road_lane.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/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index c9fa15d855268..30dd48ab383b9 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 @@ -1283,6 +1283,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) { @@ -1299,6 +1300,35 @@ void StartPlannerModule::setDebugData() const 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 = createVehicleFootprint(vehicle_info_); + 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); + } + } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (start_planner_data_.ego_predicted_path.size() > 0) { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b8a34bf5a9d00..d10fc35a5a6f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -873,7 +873,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_; @@ -938,6 +938,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}); } } @@ -1018,11 +1022,29 @@ void CrosswalkModule::updateObjectState( 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(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 7f020fbe5422c..247907ad2dc80 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -331,7 +331,7 @@ 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; 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/freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp index 8c6336b2f45d6..013eb3c33cbad 100644 --- a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/freespace_planner/test/test_freespace_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 "freespace_planner/freespace_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/mission_planner/schema/mission_planner_nodes.schema.json b/planning/mission_planner/schema/mission_planner.schema.json similarity index 96% rename from planning/mission_planner/schema/mission_planner_nodes.schema.json rename to planning/mission_planner/schema/mission_planner.schema.json index 3d0bdc41c5c34..4e302b703586d 100644 --- a/planning/mission_planner/schema/mission_planner_nodes.schema.json +++ b/planning/mission_planner/schema/mission_planner.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Mission Planner nodes", "type": "object", "definitions": { - "mission_planner_nodes": { + "mission_planner": { "type": "object", "properties": { "map_frame": { @@ -73,7 +73,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/mission_planner_nodes" + "$ref": "#/definitions/mission_planner" } }, "required": ["ros__parameters"] diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 673519b6f7a0e..ed364f0987f4f 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -111,7 +111,11 @@ geometry_msgs::msg::Pose get_closest_centerline_pose( vehicle_info_util::VehicleInfo vehicle_info) { lanelet::Lanelet closest_lanelet; - lanelet::utils::query::getClosestLanelet(road_lanelets, point, &closest_lanelet); + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets, point, &closest_lanelet, 0.0)) { + // point is not on any lanelet. + return point; + } const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); closest_lanelet.setCenterline(refined_center_line); diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 7be91d67cb945..fc347cb11f5cd 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ #define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index 37f3ce953bd48..ae638f8e4db1a 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -15,7 +15,7 @@ #include "motion_velocity_smoother/resample.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index d1694d7420ca2..84d6331b3429d 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -15,7 +15,7 @@ #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" #include diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index bb15feb32a06e..61cc4e9fb1fcc 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -15,7 +15,7 @@ #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" diff --git a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp b/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp index 4c8eaac67cbb5..430f8b78ec88c 100644 --- a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp +++ b/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_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 "motion_velocity_smoother/motion_velocity_smoother_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/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index e86782c9cb0cf..b836cee1aeaa6 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -15,7 +15,7 @@ #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp index 75b61f80622d1..9c567487e9cac 100644 --- a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp +++ b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_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 "obstacle_avoidance_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/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 6c44c4744e96c..21cd980373df2 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#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 "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 31f011b3749b9..ccfd731416475 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -15,7 +15,7 @@ #include "obstacle_cruise_planner/node.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "obstacle_cruise_planner/polygon_utils.hpp" #include "obstacle_cruise_planner/utils.hpp" diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index a7dac94e1c38a..f00ad01efb0d9 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -17,7 +17,7 @@ #include "motion_utils/distance/distance.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" diff --git a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index 5be04ab801e14..d412286d77d53 100644 --- a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_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 "obstacle_cruise_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/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index cc1ad6999b898..aea2afdb896f2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -21,7 +21,7 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include #include #include diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 369b1700aa5ca..31fb843c234d9 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -15,7 +15,7 @@ #include "obstacle_stop_planner/planner_utils.hpp" #include -#include +#include #include #include diff --git a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp index 95afdd7f2ea3b..7f986bf848777 100644 --- a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp +++ b/planning/obstacle_stop_planner/test/test_obstacle_stop_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 "obstacle_stop_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/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp index c2ffaffc75287..a89042ef210d5 100644 --- a/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp +++ b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_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 "obstacle_velocity_limiter/obstacle_velocity_limiter_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/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index d9d02a15628dd..872fc65a9d456 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -15,7 +15,7 @@ #include "path_smoother/utils/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" diff --git a/planning/path_smoother/test/test_path_smoother_node_interface.cpp b/planning/path_smoother/test/test_path_smoother_node_interface.cpp index b18c49b0e88b0..3598e07f84fd6 100644 --- a/planning/path_smoother/test/test_path_smoother_node_interface.cpp +++ b/planning/path_smoother/test/test_path_smoother_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 "path_smoother/elastic_band_smoother.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/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager.hpp similarity index 98% rename from planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp rename to planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager.hpp index 556ef9a4a4afa..a63b0d02152f0 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ -#define PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +#ifndef PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +#define PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_HPP_ // since ASSERT_NO_THROW in gtest masks the exception message, redefine it. #define ASSERT_NO_THROW_WITH_ERROR_MSG(statement) \ @@ -266,4 +266,4 @@ class PlanningInterfaceTestManager } // namespace planning_test_utils -#endif // PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +#endif // PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_HPP_ diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp similarity index 98% rename from planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp rename to planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp index 0af375c9d9c8a..4d8d7be96b44b 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ -#define PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ +#ifndef PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ +#define PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ #include "ament_index_cpp/get_package_share_directory.hpp" #include "rclcpp/rclcpp.hpp" @@ -569,4 +569,4 @@ PathWithLaneId loadPathWithLaneIdInYaml() } // namespace test_utils -#endif // PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ +#endif // PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index 6489f32dc4cdf..a9d096bf00fd4 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include namespace planning_test_utils { diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/planning_topic_converter/src/path_to_trajectory.cpp index 5362686617fc7..f5df79121029b 100644 --- a/planning/planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/planning_topic_converter/src/path_to_trajectory.cpp @@ -14,7 +14,7 @@ #include "planning_topic_converter/path_to_trajectory.hpp" -#include +#include #include namespace planning_topic_converter diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 95619e749a9f9..606dc182504a2 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,11 +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 "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" #include "planning_validator/planning_validator.hpp" +#include +#include +#include + #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index db55e5f8557e4..76fa90c6baa4b 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -15,7 +15,7 @@ #include "path_sampler/utils/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp index d3d6939f49b83..7413be07ef904 100644 --- a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp +++ b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp @@ -12,11 +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 "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" #include "scenario_selector/scenario_selector_node.hpp" +#include +#include +#include + #include #include diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index e12fde7f85abc..0aeec7be0b55d 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -18,7 +18,7 @@ #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp index ab147ab1f4f74..a8663abb2eab7 100644 --- a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp @@ -15,7 +15,7 @@ #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/system/autoware_auto_msgs_adapter/README.md b/system/autoware_auto_msgs_adapter/README.md index 85c2797074f1f..67fabaaa46c9a 100644 --- a/system/autoware_auto_msgs_adapter/README.md +++ b/system/autoware_auto_msgs_adapter/README.md @@ -21,7 +21,7 @@ The `autoware_auto_msgs_adapter` package provides the following capabilities: ## Usage -Customize the adapter configuration by replicating and editing the `adapter_control.param.yaml` file located +Customize the adapter configuration by replicating and editing the `autoware_auto_msgs_adapter_control.param.yaml` file located in the `autoware_auto_msgs_adapter/config` directory. Example configuration: ```yaml @@ -90,7 +90,7 @@ To add a new message pair, - Add a new entry to the [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) file in the `definitions:autoware_auto_msgs_adapter:properties:msg_type_target:enum` section. - Learn more about JSON schema usage in [here](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/parameters/#json-schema). - Create a new config file by replicating and editing: - - [adapter_control.param.yaml](config/adapter_control.param.yaml) + - [autoware_auto_msgs_adapter_control.param.yaml](config/autoware_auto_msgs_adapter_control.param.yaml) - Add a new test file by replicating and editing: - [test_msg_ackermann_control_command.cpp](test/test_msg_ackermann_control_command.cpp) - No need to edit the `CMakeLists.txt` file as it will automatically detect the new test file. diff --git a/system/autoware_auto_msgs_adapter/config/adapter_control.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml similarity index 100% rename from system/autoware_auto_msgs_adapter/config/adapter_control.param.yaml rename to system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml diff --git a/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml similarity index 100% rename from system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml rename to system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml diff --git a/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml similarity index 100% rename from system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml rename to system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml diff --git a/system/autoware_auto_msgs_adapter/config/adapter_planning.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml similarity index 100% rename from system/autoware_auto_msgs_adapter/config/adapter_planning.param.yaml rename to system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index a15c25f38ef3d..58f2fb4e06238 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,9 +1,9 @@ - - - - + + + + diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index a0b13a899a6ba..55af6ab2d2c55 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -30,6 +30,12 @@ contains: ["localization: localization_error_monitor"] timeout: 1.0 + localization_stability: + type: diagnostic_aggregator/GenericAnalyzer + path: localization_stability + contains: ["localization: pose_instability_detector"] + timeout: 1.0 + # This diagnostic should ideally be avoided in terms of Fault Tree Analysis (FTA) compatibility. # However, we may need this since the localization accuracy is still not reliable enough and may produce # false positives. Thus, NOTE that this diagnostic should be removed in the future when the localization accuracy diff --git a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp b/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp index 2d7ed4f3b3aa9..3648a55e9fe2c 100644 --- a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp +++ b/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp @@ -20,8 +20,9 @@ #ifndef SYSTEM_MONITOR__HDD_MONITOR__HDD_MONITOR_HPP_ #define SYSTEM_MONITOR__HDD_MONITOR__HDD_MONITOR_HPP_ +#include "system_monitor/hdd_reader/hdd_reader.hpp" + #include -#include #include #include diff --git a/system/system_monitor/include/hdd_reader/hdd_reader.hpp b/system/system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp similarity index 96% rename from system/system_monitor/include/hdd_reader/hdd_reader.hpp rename to system/system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp index a2cf8757a30f7..bb37290411c5d 100644 --- a/system/system_monitor/include/hdd_reader/hdd_reader.hpp +++ b/system/system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp @@ -17,8 +17,8 @@ * @brief HDD reader definitions */ -#ifndef HDD_READER__HDD_READER_HPP_ -#define HDD_READER__HDD_READER_HPP_ +#ifndef SYSTEM_MONITOR__HDD_READER__HDD_READER_HPP_ +#define SYSTEM_MONITOR__HDD_READER__HDD_READER_HPP_ #include #include @@ -135,4 +135,4 @@ struct UnmountDeviceInfo */ typedef std::map HddInfoList; -#endif // HDD_READER__HDD_READER_HPP_ +#endif // SYSTEM_MONITOR__HDD_READER__HDD_READER_HPP_ diff --git a/system/system_monitor/include/msr_reader/msr_reader.hpp b/system/system_monitor/include/system_monitor/msr_reader/msr_reader.hpp similarity index 90% rename from system/system_monitor/include/msr_reader/msr_reader.hpp rename to system/system_monitor/include/system_monitor/msr_reader/msr_reader.hpp index 650932891cc2f..4d8f8f49f47df 100644 --- a/system/system_monitor/include/msr_reader/msr_reader.hpp +++ b/system/system_monitor/include/system_monitor/msr_reader/msr_reader.hpp @@ -17,8 +17,8 @@ * @brief MSR reader definitions */ -#ifndef MSR_READER__MSR_READER_HPP_ -#define MSR_READER__MSR_READER_HPP_ +#ifndef SYSTEM_MONITOR__MSR_READER__MSR_READER_HPP_ +#define SYSTEM_MONITOR__MSR_READER__MSR_READER_HPP_ #include #include @@ -48,4 +48,4 @@ struct MSRInfo } }; -#endif // MSR_READER__MSR_READER_HPP_ +#endif // SYSTEM_MONITOR__MSR_READER__MSR_READER_HPP_ diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index ded9c398c6cd1..b1acac7c6ea78 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -21,7 +21,7 @@ #define SYSTEM_MONITOR__NET_MONITOR__NET_MONITOR_HPP_ #include "system_monitor/net_monitor/nl80211.hpp" -#include "traffic_reader/traffic_reader_common.hpp" +#include "system_monitor/traffic_reader/traffic_reader_common.hpp" #include diff --git a/system/system_monitor/include/traffic_reader/traffic_reader_common.hpp b/system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp similarity index 89% rename from system/system_monitor/include/traffic_reader/traffic_reader_common.hpp rename to system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp index 9d84072a42fc3..187df18735575 100644 --- a/system/system_monitor/include/traffic_reader/traffic_reader_common.hpp +++ b/system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp @@ -17,8 +17,8 @@ * @brief traffic reader definitions */ -#ifndef TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ -#define TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ +#ifndef SYSTEM_MONITOR__TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ +#define SYSTEM_MONITOR__TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ #include #include @@ -64,4 +64,4 @@ struct Result } // namespace traffic_reader_service -#endif // TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ +#endif // SYSTEM_MONITOR__TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ diff --git a/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp b/system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp similarity index 91% rename from system/system_monitor/include/traffic_reader/traffic_reader_service.hpp rename to system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp index a66d0b121ed46..0da67d95705ae 100644 --- a/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp +++ b/system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ -#define TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ +#ifndef SYSTEM_MONITOR__TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ +#define SYSTEM_MONITOR__TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ -#include "traffic_reader/traffic_reader_common.hpp" +#include "system_monitor/traffic_reader/traffic_reader_common.hpp" #include #include @@ -107,4 +107,4 @@ class TrafficReaderService } // namespace traffic_reader_service -#endif // TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ +#endif // SYSTEM_MONITOR__TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 6053c183eda4d..f12e34c8aca46 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -17,7 +17,7 @@ * @brief HDD information read class */ -#include +#include "system_monitor/hdd_reader/hdd_reader.hpp" #include #include diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index 17bbfd20a7667..16f8fe9b93477 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -17,7 +17,7 @@ * @brief MSR read class */ -#include +#include "system_monitor/msr_reader/msr_reader.hpp" #include #include diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp index 0af3b0d89d312..940afdd9e5c35 100644 --- a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "system_monitor/traffic_reader/traffic_reader_service.hpp" #include #include diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp index 592ce515f9a1f..836c4b509ffa7 100644 --- a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp @@ -17,7 +17,7 @@ * @brief traffic information read class */ -#include +#include "system_monitor/traffic_reader/traffic_reader_service.hpp" #include #include diff --git a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp index 3cc7b5f5c629c..e78d49b55a3df 100644 --- a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp +++ b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp @@ -19,10 +19,9 @@ #include "system_monitor/cpu_monitor/intel_cpu_monitor.hpp" +#include "system_monitor/msr_reader/msr_reader.hpp" #include "system_monitor/system_monitor_utility.hpp" -#include - #include #include #include diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index 91f48047a2142..c57aea81d410d 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -19,10 +19,9 @@ #include "system_monitor/hdd_monitor/hdd_monitor.hpp" +#include "system_monitor/hdd_reader/hdd_reader.hpp" #include "system_monitor/system_monitor_utility.hpp" -#include - #include #include #include diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index 07f0a6833ca16..c8775e585765e 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -20,8 +20,7 @@ #include "system_monitor/net_monitor/net_monitor.hpp" #include "system_monitor/system_monitor_utility.hpp" - -#include +#include "system_monitor/traffic_reader/traffic_reader_common.hpp" #include #include diff --git a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp b/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp index a8ede38bc52cd..fda75c21535cf 100644 --- a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp +++ b/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp @@ -19,10 +19,9 @@ #include "system_monitor/voltage_monitor/voltage_monitor.hpp" +#include "system_monitor/msr_reader/msr_reader.hpp" #include "system_monitor/system_monitor_utility.hpp" -#include - #include #include #include diff --git a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp index c6ab5de8771a4..0f321efe5c311 100644 --- a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp +++ b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "system_monitor/cpu_monitor/intel_cpu_monitor.hpp" +#include "system_monitor/msr_reader/msr_reader.hpp" -#include #include #include diff --git a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp index 375ac6083fe99..4a948029a2f3a 100644 --- a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp +++ b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "system_monitor/hdd_monitor/hdd_monitor.hpp" +#include "system_monitor/hdd_reader/hdd_reader.hpp" -#include #include #include