diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1406aee3849c0..a2daafa395cd5 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,9 +1,9 @@ ### Copied from .github/CODEOWNERS-manual ### ### Automatically generated from package.xml ### -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners +common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_common/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners -common/autoware_auto_geometry/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners +common/autoware_auto_geometry/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_tf2/** jit.ray.c@gmail.com @autowarefoundation/autoware-global-codeowners common/autoware_point_types/** taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -11,7 +11,7 @@ common/autoware_testing/** adam.dabrowski@robotec.ai @autowarefoundation/autowar common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners +common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/fake_test_node/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners common/global_parameter_loader/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -24,9 +24,10 @@ common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satosh common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners common/polar_grid/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tensorrt_common/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners +common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -65,11 +66,12 @@ evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +launch/map4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohei.sasaki@map.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners 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 yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -89,11 +91,11 @@ map/map_loader/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@ map/map_tf_generator/** azumi.suzuki@tier4.jp @autowarefoundation/autoware-global-codeowners map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners perception/bytetrack/** manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/detected_object_validation/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/detection_by_tracker/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/detected_object_validation/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners perception/euclidean_cluster/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -105,17 +107,17 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@ perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/multi_object_tracker/** jilada.eccleston@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/object_merger/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/object_range_splitter/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/probabilistic_occupancy_grid_map/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/shape_estimation/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_selector/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -131,7 +133,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 yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/obstacle_stop_planner/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -140,6 +142,10 @@ planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yuta planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners planning/scenario_selector/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/surround_obstacle_checker/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 656d632f00f8c..49da65cd85567 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -89,3 +89,11 @@ files: - source: .github/workflows/deploy-docs.yaml - source: .github/workflows/delete-closed-pr-docs.yaml + - source: mkdocs.yaml + pre-commands: | + sd "Autoware Documentation" "Autoware Universe Documentation" {source} + sd "autoware-documentation" "autoware.universe" {source} + sd "repo_url: .*" "repo_url: https://github.com/autowarefoundation/autoware.universe" {source} + sd "/edit/main/docs/" "/edit/main/" {source} + sd "docs_dir: .*" "docs_dir: ." {source} + sd "assets/(\w+)" "docs/assets/\$1" {source} diff --git a/.markdown-link-check.json b/.markdown-link-check.json index c71a3e4253687..2279120c7083b 100644 --- a/.markdown-link-check.json +++ b/.markdown-link-check.json @@ -9,6 +9,9 @@ }, { "pattern": "^https://github.com/.*/discussions/new" + }, + { + "pattern": "^https://doi.org/10.2172/7309916" } ], "retryOn429": true, diff --git a/.pages b/.pages new file mode 100644 index 0000000000000..7ee8aa0513a53 --- /dev/null +++ b/.pages @@ -0,0 +1,2 @@ +nav: + - ... | regex=^(?!.*(README.md|CONTRIBUTING.md|CODE_OF_CONDUCT.md|DISCLAIMER.md)).*$ diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp index 76fac74f667f9..62f99a4a8d8e9 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp @@ -38,6 +38,18 @@ struct SetRoute static constexpr char name[] = "/api/routing/set_route"; }; +struct ChangeRoutePoints +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; + static constexpr char name[] = "/api/routing/change_route_points"; +}; + +struct ChangeRoute +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoute; + static constexpr char name[] = "/api/routing/change_route"; +}; + struct ClearRoute { using Service = autoware_adapi_v1_msgs::srv::ClearRoute; diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml index 0659a3c923923..d4d82faf4ee78 100644 --- a/common/autoware_ad_api_specs/package.xml +++ b/common/autoware_ad_api_specs/package.xml @@ -7,12 +7,11 @@ Takagi, Isamu yabuta Kah Hooi Tan - Kenji Miyake + Ryohsuke Mitsudome Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake ament_lint_auto autoware_lint_common diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml index 17d666e2273e6..59e2907e91501 100644 --- a/common/autoware_auto_common/package.xml +++ b/common/autoware_auto_common/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake builtin_interfaces eigen diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md index dc4b33b56b4bc..f651c218bc80d 100644 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md @@ -12,7 +12,7 @@ association and in any application that deals with the objects around the percei ## Design -[\(Livermore, Calif, 1977\)](https://www.osti.gov/biblio/7309916/) mention the following +[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following observations about convex polygon intersection: - Intersection of two convex polygons is a convex polygon diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml index e19ac4897aab0..f53412298a485 100644 --- a/common/autoware_auto_geometry/package.xml +++ b/common/autoware_auto_geometry/package.xml @@ -4,12 +4,13 @@ autoware_auto_geometry 1.0.0 Geometry related algorithms - Apex.AI, Inc. + Satoshi Ota Apache License 2.0 - ament_cmake_auto + Apex.AI, Inc. - autoware_cmake + ament_cmake_auto + autoware_cmake autoware_auto_common autoware_auto_geometry_msgs 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 acf4a389ec6e3..a37aa79288177 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 @@ -81,15 +81,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_default_topic{default_topic} { m_display_type_property = new rviz_common::properties::EnumProperty( - "Polygon Type", "3d", "Type of the polygon to display object.", this, SLOT(updatePalette())); + "Polygon Type", "3d", "Type of the polygon to display object."); // Option values here must correspond to indices in palette_textures_ array in onInitialize() // below. m_display_type_property->addOption("3d", 0); m_display_type_property->addOption("2d", 1); m_display_type_property->addOption("Disable", 2); m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( - "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this, - SLOT(updatePalette())); + "Visualization Type", "Normal", "Simplicity of the polygon to display object."); 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. diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index 4771589dc9038..c1ad3e3df140e 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -12,8 +12,8 @@ Apache 2.0 ament_cmake + autoware_cmake - autoware_cmake libboost-dev qtbase5-dev diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md index 3befe575e3e7f..aaa719d617373 100644 --- a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md +++ b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md @@ -8,7 +8,7 @@ In general, users of ROS rely on tf (and its successor, tf2) for publishing and frame transforms. This is true even to the extent that the tf2 contains the packages `tf2_geometry_msgs` and `tf2_sensor_msgs` which allow for easy conversion to and from the message types defined in `geometry_msgs` and `sensor_msgs`, respectively. However, AutowareAuto contains -some specialized message types which are not transformable between frames using the ROS2 library. +some specialized message types which are not transformable between frames using the ROS 2 library. The `autoware_auto_tf2` package aims to provide developers with tools to transform applicable `autoware_auto_msgs` types. In addition to this, this package also provides transform tools for messages types in `geometry_msgs` missing in `tf2_geometry_msgs`. @@ -49,7 +49,7 @@ const ros::Time& tf2::getTimestamp(const T& t); ## Current Implementation of tf2_geometry_msgs -In both ROS1 and ROS2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated +In both ROS 1 and ROS 2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated functions like: - `getTimestamp` @@ -58,13 +58,13 @@ functions like: - `toMsg` - `fromMsg` -In ROS1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped +In ROS 1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped underlying data like `Vector3`, `Point`, have implementations of the following functions: - `toMsg` - `fromMsg` -In ROS2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 +In ROS 2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data were not added; such as `Vector3`, `Point`, or ported in this commit ros/geometry2/commit/6f2a82. The non-stamped data with `toMsg` and `fromMsg` are `Quaternion`, `Transform`. `Pose` has the diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml index c80ce45a217ac..c7b620a9a737e 100644 --- a/common/autoware_auto_tf2/package.xml +++ b/common/autoware_auto_tf2/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_common autoware_auto_geometry_msgs diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml index 8829bd7538d9b..e05b604e90d13 100644 --- a/common/autoware_point_types/package.xml +++ b/common/autoware_point_types/package.xml @@ -10,12 +10,11 @@ ament_cmake_core ament_cmake_export_dependencies ament_cmake_test + autoware_cmake ament_cmake_core ament_cmake_test - autoware_cmake - ament_cmake_copyright ament_cmake_cppcheck ament_cmake_lint_cmake diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml index 30a27bb9435b1..769942b437884 100644 --- a/common/autoware_testing/package.xml +++ b/common/autoware_testing/package.xml @@ -9,8 +9,7 @@ ament_cmake_auto ament_cmake_lint_cmake - - autoware_cmake + autoware_cmake ros_testing diff --git a/common/bag_time_manager_rviz_plugin/package.xml b/common/bag_time_manager_rviz_plugin/package.xml index b12c24b728cde..f82d75b46e06f 100644 --- a/common/bag_time_manager_rviz_plugin/package.xml +++ b/common/bag_time_manager_rviz_plugin/package.xml @@ -8,8 +8,8 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake libqt5-core libqt5-gui libqt5-widgets diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 54a0f1a66d5d2..d4fe460a6daea 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml index a1eba3cb41e09..ebf0684d0066c 100644 --- a/common/component_interface_tools/package.xml +++ b/common/component_interface_tools/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_updater fmt diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index 90602a65bd1b0..a1803a35cc1c9 100644 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -7,12 +7,12 @@ Takagi, Isamu yabuta Kah Hooi Tan - Kenji Miyake + Yukihiro Saito Apache License 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake tier4_system_msgs autoware_adapi_v1_msgs diff --git a/common/cuda_utils/package.xml b/common/cuda_utils/package.xml index 53127da0c1ff6..4ae44469f1efa 100644 --- a/common/cuda_utils/package.xml +++ b/common/cuda_utils/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake ament_lint_auto autoware_lint_common diff --git a/common/fake_test_node/package.xml b/common/fake_test_node/package.xml index 8e919d719fc6c..78d53831670d8 100644 --- a/common/fake_test_node/package.xml +++ b/common/fake_test_node/package.xml @@ -8,8 +8,7 @@ Apache 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake ament_cmake_ros autoware_auto_common diff --git a/common/global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml index 97e679334e6c2..5fa46351930cf 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake vehicle_info_util diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index 941186daa063b..49cb674f0a256 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs geometry_msgs diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 5a39d818a3238..e5d2c7af8d06e 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake tier4_autoware_utils diff --git a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp index 2ae8877aeea60..79a47bde3a1b2 100644 --- a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp +++ b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp @@ -15,8 +15,8 @@ #ifndef KALMAN_FILTER__KALMAN_FILTER_HPP_ #define KALMAN_FILTER__KALMAN_FILTER_HPP_ -#include -#include +#include +#include /** * @file kalman_filter.h diff --git a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp index 1a072bb43d4ac..cdc03f3558854 100644 --- a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp +++ b/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp @@ -17,8 +17,8 @@ #include "kalman_filter/kalman_filter.hpp" -#include -#include +#include +#include #include diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index f75755c172605..ea061f3f23bb8 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -12,8 +12,8 @@ Takamasa Horibe ament_cmake_auto + autoware_cmake - autoware_cmake eigen eigen3_cmake_module diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 166d3a6661689..1328f1fada9a7 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -28,15 +28,15 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); @@ -58,33 +58,36 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, + const std::string & ns_prefix)>; using delete_wall_function = std::function; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const std::vector & stop_poses, const std::string & module_name, const rclcpp::Time & now, - int32_t id, const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const std::vector & slow_down_poses, const std::string & module_name, - const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0); + const rclcpp::Time & now, const double longitudinal_offset = 0.0, + const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const std::vector & dead_line_poses, const std::string & module_name, - const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0); + const rclcpp::Time & now, const double longitudinal_offset = 0.0, + const std::string & ns_prefix = ""); private: visualization_msgs::msg::MarkerArray handleVirtualWallMarker( const std::vector & poses, const std::string & module_name, const rclcpp::Time & now, - int32_t id, create_wall_function function_create_wall_marker, - delete_wall_function function_delete_wall_marker, - std::vector & previous_poses, const double longitudinal_offset = 0.0); + create_wall_function function_create_wall_marker, + delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); - std::vector previous_stop_poses_; - std::vector previous_slow_down_poses_; - std::vector previous_dead_line_poses_; + size_t previous_stop_poses_nb_ = 0UL; + size_t previous_slow_down_poses_nb_ = 0UL; + size_t previous_dead_line_poses_nb_ = 0UL; }; } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 9a2527d1be4a5..c65a7eae189e3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1107,17 +1107,27 @@ inline boost::optional insertTargetPoint( { validateNonEmpty(points); - if (insert_point_length < 0.0 || src_segment_idx >= points.size() - 1) { + if (src_segment_idx >= points.size() - 1) { return boost::none; } // Get Nearest segment index boost::optional segment_idx = boost::none; - for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { - const double length = calcSignedArcLength(points, src_segment_idx, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; + if (0.0 <= insert_point_length) { + for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + } else { + for (int i = src_segment_idx - 1; 0 <= i; --i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (length <= insert_point_length) { + segment_idx = i; + break; + } } } @@ -1128,7 +1138,7 @@ inline boost::optional insertTargetPoint( // Get Target Point const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); const double target_length = - std::max(0.0, insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx)); + insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); const auto p_target = tier4_autoware_utils::calcInterpolatedPoint( tier4_autoware_utils::getPoint(points.at(*segment_idx)), @@ -1728,6 +1738,76 @@ T cropPoints( return cropped_points; } + +/** + * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. + * The function gets the nearest segment index between the points of the trajectory and the given + * pose's position, then calculates the azimuth angle of that segment and compares it to the yaw of + * the input pose. The segment is a straight path between two continuous points of the trajectory. + * @param points Points of the trajectory, path, ... + * @param pose Input pose with position and orientation (yaw) + * @param throw_exception Flag to enable/disable exception throwing + * @return Angle with respect to the trajectory segment (signed) in radians + */ +template +double calcYawDeviation( + const T & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } + } + + if (overlap_removed_points.size() <= 1) { + const std::runtime_error e("points size is less than 2"); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return 0.0; + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); + + const double path_yaw = tier4_autoware_utils::calcAzimuthAngle( + tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)), + tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); + const double pose_yaw = tf2::getYaw(pose.orientation); + + return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); +} + +/** + * @brief Check if the given target point is in front of the based pose from the trajectory. + * if the points is empty, the function returns false + * @param points Points of the trajectory, path, ... + * @param base_point Base point + * @param target_point Target point + * @param threshold threshold for judging front point + * @return true if the target pose is in front of the base pose + */ +template +bool isTargetPointFront( + const T & points, const geometry_msgs::msg::Point & base_point, + const geometry_msgs::msg::Point & target_point, const double threshold = 0.0) +{ + if (points.empty()) { + return false; + } + + const double s_base = calcSignedArcLength(points, 0, base_point); + const double s_target = calcSignedArcLength(points, 0, target_point); + + return s_target - s_base > threshold; +} } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index ee6975e0caa83..834342624bfcd 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -20,8 +20,7 @@ Satoshi Ota ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs autoware_auto_vehicle_msgs diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index 6349d724c635c..14ba02ab38703 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/motion_utils/src/distance/distance.cpp @@ -88,12 +88,12 @@ std::tuple update( * @param (am) minimum deceleration [m/ss] * @param (ja) maximum jerk [m/sss] * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] + * @param (t_during_min_acc) duration of constant deceleration [s] * @return moving distance until velocity is reached vt [m] */ boost::optional calcDecelDistPlanType1( const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_min) + const double jd, const double t_during_min_acc) { constexpr double epsilon = 1e-3; @@ -103,7 +103,7 @@ boost::optional calcDecelDistPlanType1( const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); // zero jerk time - const double t2 = epsilon < t_min ? t_min : 0.0; + const double t2 = epsilon < t_during_min_acc ? t_during_min_acc : 0.0; const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); // positive jerk time @@ -238,22 +238,30 @@ boost::optional calcDecelDistWithJerkAndAccConstraints( const double current_vel, const double target_vel, const double current_acc, const double acc_min, const double jerk_acc, const double jerk_dec) { + if (current_vel < target_vel) { + return {}; + } + constexpr double epsilon = 1e-3; - const double t_dec = - acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; - const double t_acc = (0.0 - acc_min) / jerk_acc; - const double t_min = (target_vel - current_vel - current_acc * t_dec - - 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / - acc_min; + const double jerk_before_min_acc = acc_min < current_acc ? jerk_dec : jerk_acc; + const double t_before_min_acc = (acc_min - current_acc) / jerk_before_min_acc; + const double jerk_after_min_acc = jerk_acc; + const double t_after_min_acc = (0.0 - acc_min) / jerk_after_min_acc; + + const double t_during_min_acc = + (target_vel - current_vel - current_acc * t_before_min_acc - + 0.5 * jerk_before_min_acc * std::pow(t_before_min_acc, 2) - acc_min * t_after_min_acc - + 0.5 * jerk_after_min_acc * std::pow(t_after_min_acc, 2)) / + acc_min; // check if it is possible to decelerate to the target velocity // by simply bringing the current acceleration to zero. const auto is_decel_needed = 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - if (t_min > epsilon) { + if (t_during_min_acc > epsilon) { return calcDecelDistPlanType1( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); } else if (is_decel_needed || current_acc > epsilon) { return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); } diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index ab4dc623c3125..bc6a938400b4b 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -86,32 +86,35 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) { const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); return createVirtualWallMarkerArray( - pose_with_offset, module_name, "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); + pose_with_offset, module_name, ns_prefix + "stop_", now, id, + createMarkerColor(1.0, 0.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) { const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); return createVirtualWallMarkerArray( - pose_with_offset, module_name, "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); + pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, + createMarkerColor(1.0, 1.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) { const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); return createVirtualWallMarkerArray( - pose_with_offset, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); + pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, + createMarkerColor(0.0, 1.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( @@ -134,76 +137,63 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker( const std::vector & poses, const std::string & module_name, const rclcpp::Time & now, - int32_t id, create_wall_function function_create_wall_marker, - delete_wall_function function_delete_wall_marker, - std::vector & previous_virtual_wall_poses, - const double longitudinal_offset) + create_wall_function function_create_wall_marker, + delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb, + const double longitudinal_offset, const std::string & ns_prefix) { - size_t id_to_create = id; - size_t id_to_delete = id; visualization_msgs::msg::MarkerArray wall_marker; - if (poses.size() == 0) { - return wall_marker; - } - - for (const auto & p : previous_virtual_wall_poses) { - const bool previous_stop_pose_is_in_stop_pose = - std::any_of(poses.begin(), poses.end(), [&](const geometry_msgs::msg::Pose & elem) { - std::vector poses; - poses.push_back(p); - poses.push_back(elem); - return resample_utils::validate_points_duplication(poses); - }); - - if (!previous_stop_pose_is_in_stop_pose) { - appendMarkerArray(function_delete_wall_marker(now, id_to_delete), &wall_marker, now); - } - id_to_delete++; - } + int32_t id = 0; + const auto max_id = static_cast(previous_virtual_walls_nb); for (const auto & p : poses) { appendMarkerArray( - function_create_wall_marker(p, module_name, now, id_to_create++, longitudinal_offset), + function_create_wall_marker(p, module_name, now, id++, longitudinal_offset, ns_prefix), &wall_marker); } - previous_virtual_wall_poses = poses; + + while (id < max_id) { + appendMarkerArray(function_delete_wall_marker(now, id++), &wall_marker, now); + } + + previous_virtual_walls_nb = poses.size(); return wall_marker; } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker( const std::vector & stop_poses, const std::string & module_name, const rclcpp::Time & now, - int32_t id, const double longitudinal_offset) + const double longitudinal_offset, const std::string & ns_prefix) { create_wall_function creator = motion_utils::createStopVirtualWallMarker; delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker; return handleVirtualWallMarker( - stop_poses, module_name, now, id, creator, deleter, previous_stop_poses_, longitudinal_offset); + stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset, + ns_prefix); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker( const std::vector & slow_down_poses, const std::string & module_name, - const rclcpp::Time & now, int32_t id, const double longitudinal_offset) + const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix) { create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker; delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker; return handleVirtualWallMarker( - slow_down_poses, module_name, now, id, creator, deleter, previous_slow_down_poses_, - longitudinal_offset); + slow_down_poses, module_name, now, creator, deleter, previous_slow_down_poses_nb_, + longitudinal_offset, ns_prefix); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker( const std::vector & dead_line_poses, const std::string & module_name, - const rclcpp::Time & now, int32_t id, const double longitudinal_offset) + const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix) { create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker; delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker; return handleVirtualWallMarker( - dead_line_poses, module_name, now, id, creator, deleter, previous_dead_line_poses_, - longitudinal_offset); + dead_line_poses, module_name, now, creator, deleter, previous_dead_line_poses_nb_, + longitudinal_offset, ns_prefix); } } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 107a77393d5a2..091f3405e2815 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -188,21 +188,21 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i); - for (size_t i = 1; i < resampling_arclength.size(); ++i) { + for (size_t j = 1; j < resampling_arclength.size(); ++j) { if ( - resampling_arclength.at(i - 1) <= distance_to_resampling_point && - distance_to_resampling_point < resampling_arclength.at(i)) { + resampling_arclength.at(j - 1) <= distance_to_resampling_point && + distance_to_resampling_point < resampling_arclength.at(j)) { const double dist_to_prev_point = - std::fabs(distance_to_resampling_point - resampling_arclength.at(i - 1)); + std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1)); const double dist_to_following_point = - std::fabs(resampling_arclength.at(i) - distance_to_resampling_point); + std::fabs(resampling_arclength.at(j) - distance_to_resampling_point); if (dist_to_prev_point < motion_utils::overlap_threshold) { - resampling_arclength.at(i - 1) = distance_to_resampling_point; + resampling_arclength.at(j - 1) = distance_to_resampling_point; } else if (dist_to_following_point < motion_utils::overlap_threshold) { - resampling_arclength.at(i) = distance_to_resampling_point; + resampling_arclength.at(j) = distance_to_resampling_point; } else { resampling_arclength.insert( - resampling_arclength.begin() + i, distance_to_resampling_point); + resampling_arclength.begin() + j, distance_to_resampling_point); } break; } diff --git a/common/motion_utils/test/src/distance/test_distance.cpp b/common/motion_utils/test/src/distance/test_distance.cpp index b5b9c5cfdc95f..5bcb2c26fe1bd 100644 --- a/common/motion_utils/test/src/distance/test_distance.cpp +++ b/common/motion_utils/test/src/distance/test_distance.cpp @@ -37,21 +37,6 @@ TEST(distance, calcDecelDistWithJerkAndAccConstraints) EXPECT_FALSE(dist); } - // invalid deceleration - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 0.0; - constexpr double current_acc = -2.5; - constexpr double acc_min = -0.5; - constexpr double jerk_acc = 1.0; - constexpr double jerk_dec = -0.5; - - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - - EXPECT_FALSE(dist); - } - // normal stop { constexpr double current_vel = 16.7; @@ -111,6 +96,21 @@ TEST(distance, calcDecelDistWithJerkAndAccConstraints) current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); EXPECT_NEAR(expected_dist, *dist, epsilon); } + + // current_acc is lower than acc_min + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = -2.5; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + constexpr double expected_dist = 217.429; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } } } // namespace diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index ccbe0f75ee267..6237813a8084c 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -2750,6 +2750,193 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } } +TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) +{ + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::getPose; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Insert + for (double x_start = -0.5; x_start < -5.0; x_start -= 1.0) { + auto traj_out = traj; + + const size_t start_idx = 7; + const auto p_target = createPoint(7.0 + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = -1.0 - 1e-2; + + const size_t start_idx = 8; + const auto p_target = createPoint(7.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + const double x_start = -1.0; + + const size_t start_idx = 8; + const auto p_target = createPoint(7.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = -5.0; + + const size_t start_idx = 5; + const auto p_target = createPoint(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Index Out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), boost::none); + } +} + TEST(trajectory, insertTargetPoint_Length_from_a_pose) { using motion_utils::calcArcLength; @@ -5238,3 +5425,128 @@ TEST(Trajectory, removeInvalidOrientationPoints) }, 1); // expected size is 1 since only the first point remains } + +TEST(trajectory, calcYawDeviation) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using motion_utils::calcYawDeviation; + + constexpr double tolerance = 1e-3; + + // Generate test trajectory + const auto trajectory = generateTestTrajectory(4, 10.0, 0.0, 0.0, M_PI / 8); + + // check with fist point + { + const auto input_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose); + constexpr double expected_yaw_deviation = -M_PI / 8; + EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); + } + + // check with middle point + { + const auto input_pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, M_PI / 8); + const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose); + constexpr double expected_yaw_deviation = -0.734; + EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); + } +} + +TEST(trajectory, isTargetPointFront) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using motion_utils::isTargetPointFront; + using tier4_autoware_utils::createPoint; + + // Generate test trajectory + const auto trajectory = generateTestTrajectory(10, 1.0); + + // Front point is base + { + const auto base_point = createPoint(5.0, 0.0, 0.0); + const auto target_point = createPoint(1.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // Front point is target + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(3.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // boundary condition + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(1.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // before the start point + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(-3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + { + const auto base_point = createPoint(-5.0, 0.0, 0.0); + const auto target_point = createPoint(1.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // after the end point + { + const auto base_point = createPoint(10.0, 0.0, 0.0); + const auto target_point = createPoint(3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + { + const auto base_point = createPoint(2.0, 0.0, 0.0); + const auto target_point = createPoint(14.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // empty point + { + const Trajectory traj; + const auto base_point = createPoint(2.0, 0.0, 0.0); + const auto target_point = createPoint(5.0, 0.0, 0.0); + EXPECT_FALSE(isTargetPointFront(traj.points, base_point, target_point)); + } + + // non-default threshold + { + const double threshold = 3.0; + + { + const auto base_point = createPoint(5.0, 0.0, 0.0); + const auto target_point = createPoint(3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(4.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(4.1, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + } +} diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp index cabced5855d10..a919bc1f1c038 100644 --- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp +++ b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp @@ -15,10 +15,11 @@ #ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ #define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ -#include "eigen3/Eigen/Core" #include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') #include "osqp_interface/visibility_control.hpp" +#include + #include namespace autoware diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index fa9d491cd9c5f..f126ba9329d3e 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -15,11 +15,11 @@ #ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_ #define OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#include "eigen3/Eigen/Core" #include "osqp/osqp.h" #include "osqp_interface/csc_matrix_conv.hpp" #include "osqp_interface/visibility_control.hpp" +#include #include #include diff --git a/common/osqp_interface/package.xml b/common/osqp_interface/package.xml index 643611ca48448..41ee5bb9055a6 100644 --- a/common/osqp_interface/package.xml +++ b/common/osqp_interface/package.xml @@ -11,8 +11,7 @@ Apache 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake eigen osqp_vendor diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/osqp_interface/src/csc_matrix_conv.cpp index 051740e8974e1..1944face4516b 100644 --- a/common/osqp_interface/src/csc_matrix_conv.cpp +++ b/common/osqp_interface/src/csc_matrix_conv.cpp @@ -14,8 +14,8 @@ #include "osqp_interface/csc_matrix_conv.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/SparseCore" +#include +#include #include #include diff --git a/common/osqp_interface/test/test_csc_matrix_conv.cpp b/common/osqp_interface/test/test_csc_matrix_conv.cpp index 50b8487c26d80..765f1a1afed3b 100644 --- a/common/osqp_interface/test/test_csc_matrix_conv.cpp +++ b/common/osqp_interface/test/test_csc_matrix_conv.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "eigen3/Eigen/Core" #include "gtest/gtest.h" #include "osqp_interface/csc_matrix_conv.hpp" +#include + #include #include #include diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index cf96f0164bafc..caa89c79d08ea 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "eigen3/Eigen/Core" #include "gtest/gtest.h" #include "osqp_interface/osqp_interface.hpp" +#include + #include #include diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index 997fbec2d4ef5..b5e770a0ea624 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs motion_utils diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 5d1fa5cae84a6..bc1d1b8b6bece 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs geometry_msgs diff --git a/common/polar_grid/package.xml b/common/polar_grid/package.xml index 75118d09ef5d2..ece156669c85b 100644 --- a/common/polar_grid/package.xml +++ b/common/polar_grid/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libqt5-core libqt5-gui diff --git a/common/qp_interface/CMakeLists.txt b/common/qp_interface/CMakeLists.txt new file mode 100644 index 0000000000000..00b2847309e9e --- /dev/null +++ b/common/qp_interface/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.14) +project(qp_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) +find_package(proxsuite REQUIRED) + +# after find_package(osqp_vendor) in ament_auto_find_build_dependencies +find_package(osqp REQUIRED) +get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) +get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) + +set(QP_INTERFACE_LIB_SRC + src/qp_interface.cpp + src/osqp_interface.cpp + src/osqp_csc_matrix_conv.cpp + src/proxqp_interface.cpp +) + +set(QP_INTERFACE_LIB_HEADERS + include/qp_interface/qp_interface.hpp + include/qp_interface/osqp_interface.hpp + include/qp_interface/osqp_csc_matrix_conv.hpp + include/qp_interface/proxqp_interface.hpp +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${QP_INTERFACE_LIB_SRC} + ${QP_INTERFACE_LIB_HEADERS} +) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) + +target_include_directories(qp_interface + SYSTEM PUBLIC + "${OSQP_INCLUDE_DIR}" + "${EIGEN3_INCLUDE_DIR}" +) + +ament_target_dependencies(qp_interface + Eigen3 + osqp_vendor + proxsuite +) + +# crucial so downstream package builds because osqp_interface exposes osqp.hpp +ament_export_include_directories("${OSQP_INCLUDE_DIR}") +# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a +ament_export_libraries(osqp::osqp) + +if(BUILD_TESTING) + set(TEST_SOURCES + test/test_osqp_interface.cpp + test/test_csc_matrix_conv.cpp + test/test_proxqp_interface.cpp + ) + set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) + ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) + target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/common/qp_interface/design/qp_interface-design.md b/common/qp_interface/design/qp_interface-design.md new file mode 100644 index 0000000000000..3843e5a742d1e --- /dev/null +++ b/common/qp_interface/design/qp_interface-design.md @@ -0,0 +1,48 @@ +# Interface for QP solvers + +This is the design document for the `qp_interface` package. + +## Purpose / Use cases + +This packages provides a C++ interface for QP solvers. +Currently, supported QP solvers are + +- [OSQP library](https://osqp.org/docs/solver/index.html) + +## Design + +The class `QPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into +C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the QP workspace dataholder, and runs the optimizer. + +## Inputs / Outputs / API + +The interface can be used in several ways: + +1. Initialize the interface, and load the problem formulation at the optimization call. + + ```cpp + QPInterface qp_interface; + qp_interface.optimize(P, A, q, l, u); + ``` + +2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. + + ```cpp + QPInterface qp_interface(true); + qp_interface.optimize(P, A, q, l, u); + qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); + ``` + + The optimization results are returned as a vector by the optimization function. + + ```cpp + const auto solution = qp_interface.optimize(); + double x_0 = solution[0]; + double x_1 = solution[1]; + ``` + +## References / External links + +- OSQP library: + +## Related issues diff --git a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp new file mode 100644 index 0000000000000..74ec4c1282f46 --- /dev/null +++ b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ +#define QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ + +#include "osqp/glob_opts.h" + +#include + +#include + +namespace qp +{ +/// \brief Compressed-Column-Sparse Matrix +struct CSC_Matrix +{ + /// Vector of non-zero values. Ex: [4,1,1,2] + std::vector m_vals; + /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') + std::vector m_row_idxs; + /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') + std::vector m_col_idxs; +}; + +/// \brief Calculate CSC matrix from Eigen matrix +CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); +/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix +CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); +/// \brief Print the given CSC matrix to the standard output +void printCSCMatrix(const CSC_Matrix & csc_mat); + +} // namespace qp + +#endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp new file mode 100644 index 0000000000000..ef2c3bd17c89e --- /dev/null +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -0,0 +1,145 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef QP_INTERFACE__OSQP_INTERFACE_HPP_ +#define QP_INTERFACE__OSQP_INTERFACE_HPP_ + +#include "osqp/osqp.h" +#include "qp_interface/osqp_csc_matrix_conv.hpp" +#include "qp_interface/qp_interface.hpp" + +#include +#include +#include +#include + +namespace qp +{ +constexpr c_float INF = 1e30; + +class OSQPInterface : public QPInterface +{ +public: + /// \brief Constructor without problem formulation + OSQPInterface( + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); + /// \brief Constructor with problem setup + /// \param P: (n,n) matrix defining relations between parameters. + /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. + /// \param q: (n) vector defining the linear cost of the problem. + /// \param l: (m) vector defining the lower bound problem constraint. + /// \param u: (m) vector defining the upper bound problem constraint. + /// \param eps_abs: Absolute convergence tolerance. + OSQPInterface( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon()); + OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon()); + ~OSQPInterface(); + + static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; + + std::vector optimize( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + int getIteration() const override; + int getStatus() const override; + int getPolishStatus() const; + std::vector getDualSolution() const; + + void updateEpsAbs(const double eps_abs) override; + void updateEpsRel(const double eps_rel) override; + void updateVerbose(const bool verbose) override; + + // Updates problem parameters while keeping solution in memory. + // + // Args: + // P_new: (n,n) matrix defining relations between parameters. + // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. + // q_new: (n) vector defining the linear cost of the problem. + // l_new: (m) vector defining the lower bound problem constraint. + // u_new: (m) vector defining the upper bound problem constraint. + void updateP(const Eigen::MatrixXd & P_new); + void updateCscP(const CSC_Matrix & P_csc); + void updateA(const Eigen::MatrixXd & A_new); + void updateCscA(const CSC_Matrix & A_csc); + void updateQ(const std::vector & q_new); + void updateL(const std::vector & l_new); + void updateU(const std::vector & u_new); + void updateBounds(const std::vector & l_new, const std::vector & u_new); + + void updateMaxIter(const int iter); + void updateRhoInterval(const int rho_interval); + void updateRho(const double rho); + void updateAlpha(const double alpha); + void updateScaling(const int scaling); + void updatePolish(const bool polish); + void updatePolishRefinementIteration(const int polish_refine_iter); + void updateCheckTermination(const int check_termination); + + /// \brief Get the number of iteration taken to solve the problem + inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } + /// \brief Get the status message for the latest problem solved + inline std::string getStatusMessage() const + { + return static_cast(m_latest_work_info.status); + } + /// \brief Get the runtime of the latest problem solved + inline double getRunTime() const { return m_latest_work_info.run_time; } + /// \brief Get the objective value the latest problem solved + inline double getObjVal() const { return m_latest_work_info.obj_val; } + /// \brief Returns flag asserting interface condition (Healthy condition: 0). + inline int64_t getExitFlag() const { return m_exitflag; } + + void logUnsolvedStatus(const std::string & prefix_message = "") const; + + // Setter functions for warm start + bool setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables); + bool setPrimalVariables(const std::vector & primal_variables); + bool setDualVariables(const std::vector & dual_variables); + +private: + std::unique_ptr> m_work; + std::unique_ptr m_settings; + std::unique_ptr m_data; + // store last work info since work is cleaned up at every execution to prevent memory leak. + OSQPInfo m_latest_work_info; + // Number of parameters to optimize + int64_t m_param_n; + // Flag to check if the current work exists + bool m_work_initialized = false; + // Exitflag + int64_t m_exitflag; + + void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) override; + + void initializeCSCProblemImpl( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + std::vector optimizeImpl() override; +}; +} // namespace qp + +#endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp new file mode 100644 index 0000000000000..a940262d5f6da --- /dev/null +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef QP_INTERFACE__PROXQP_INTERFACE_HPP_ +#define QP_INTERFACE__PROXQP_INTERFACE_HPP_ + +#include "qp_interface/qp_interface.hpp" + +#include +#include + +#include +#include +#include + +namespace qp +{ +class ProxQPInterface : public QPInterface +{ +public: + explicit ProxQPInterface( + const bool enable_warm_start = false, + const double eps_abs = std::numeric_limits::epsilon()); + + int getIteration() const override; + int getStatus() const override; + + void updateEpsAbs(const double eps_abs) override; + void updateEpsRel(const double eps_rel) override; + void updateVerbose(const bool verbose) override; + +private: + proxsuite::proxqp::Settings m_settings; + std::shared_ptr> m_qp_ptr; + + void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) override; + + std::vector optimizeImpl() override; +}; +} // namespace qp + +#endif // QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/qp_interface.hpp b/common/qp_interface/include/qp_interface/qp_interface.hpp new file mode 100644 index 0000000000000..026c0fe413b71 --- /dev/null +++ b/common/qp_interface/include/qp_interface/qp_interface.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef QP_INTERFACE__QP_INTERFACE_HPP_ +#define QP_INTERFACE__QP_INTERFACE_HPP_ + +#include + +#include +#include + +namespace qp +{ +class QPInterface +{ +public: + explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {} + + std::vector optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + virtual int getIteration() const = 0; + virtual int getStatus() const = 0; + + virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; + virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; + virtual void updateVerbose([[maybe_unused]] const bool verbose) {} + +protected: + bool m_enable_warm_start; + + void initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + virtual void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) = 0; + + virtual std::vector optimizeImpl() = 0; + + std::optional m_variables_num; + std::optional m_constraints_num; +}; +} // namespace qp + +#endif // QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/qp_interface/package.xml b/common/qp_interface/package.xml new file mode 100644 index 0000000000000..22cc3665ffaa0 --- /dev/null +++ b/common/qp_interface/package.xml @@ -0,0 +1,30 @@ + + + + qp_interface + 1.0.0 + Interface for the QP solvers + Takayuki Murooka + Fumiya Watanabe + Maxime CLEMENT + Satoshi Ota + Apache 2.0 + + ament_cmake_auto + autoware_cmake + + eigen + osqp_vendor + proxsuite + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/common/qp_interface/src/osqp_csc_matrix_conv.cpp b/common/qp_interface/src/osqp_csc_matrix_conv.cpp new file mode 100644 index 0000000000000..0faf7586463fd --- /dev/null +++ b/common/qp_interface/src/osqp_csc_matrix_conv.cpp @@ -0,0 +1,134 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "qp_interface/osqp_csc_matrix_conv.hpp" + +#include +#include + +#include +#include +#include + +namespace qp +{ +CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) +{ + const size_t elem = static_cast(mat.nonZeros()); + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + std::vector vals; + vals.reserve(elem); + std::vector row_idxs; + row_idxs.reserve(elem); + std::vector col_idxs; + col_idxs.reserve(elem); + + // Construct CSC matrix arrays + c_float val; + c_int elem_count = 0; + + col_idxs.push_back(0); + + for (Eigen::Index j = 0; j < cols; j++) { // col iteration + for (Eigen::Index i = 0; i < rows; i++) { // row iteration + // Get values of nonzero elements + val = mat(i, j); + if (std::fabs(val) < 1e-9) { + continue; + } + elem_count += 1; + + // Store values + vals.push_back(val); + row_idxs.push_back(i); + } + + col_idxs.push_back(elem_count); + } + + CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; + + return csc_matrix; +} + +CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) +{ + const size_t elem = static_cast(mat.nonZeros()); + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + if (rows != cols) { + throw std::invalid_argument("Matrix must be square (n, n)"); + } + + std::vector vals; + vals.reserve(elem); + std::vector row_idxs; + row_idxs.reserve(elem); + std::vector col_idxs; + col_idxs.reserve(elem); + + // Construct CSC matrix arrays + c_float val; + Eigen::Index trap_last_idx = 0; + c_int elem_count = 0; + + col_idxs.push_back(0); + + for (Eigen::Index j = 0; j < cols; j++) { // col iteration + for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration + // Get values of nonzero elements + val = mat(i, j); + if (std::fabs(val) < 1e-9) { + continue; + } + elem_count += 1; + + // Store values + vals.push_back(val); + row_idxs.push_back(i); + } + + col_idxs.push_back(elem_count); + trap_last_idx += 1; + } + + CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; + + return csc_matrix; +} + +void printCSCMatrix(const CSC_Matrix & csc_mat) +{ + std::cout << "["; + for (const c_float & val : csc_mat.m_vals) { + std::cout << val << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & row : csc_mat.m_row_idxs) { + std::cout << row << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & col : csc_mat.m_col_idxs) { + std::cout << col << ", "; + } + std::cout << "]\n"; +} +} // namespace qp diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp new file mode 100644 index 0000000000000..81d2ffeeaaed5 --- /dev/null +++ b/common/qp_interface/src/osqp_interface.cpp @@ -0,0 +1,399 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "qp_interface/osqp_interface.hpp" + +#include "qp_interface/osqp_csc_matrix_conv.hpp" + +#include + +namespace qp +{ +OSQPInterface::OSQPInterface(const bool enable_warm_start, const c_float eps_abs, const bool polish) +: QPInterface(enable_warm_start), m_work{nullptr, OSQPWorkspaceDeleter} +{ + m_settings = std::make_unique(); + m_data = std::make_unique(); + + if (m_settings) { + osqp_set_default_settings(m_settings.get()); + m_settings->alpha = 1.6; // Change alpha parameter + m_settings->eps_rel = 1.0E-4; + m_settings->eps_abs = eps_abs; + m_settings->eps_prim_inf = 1.0E-4; + m_settings->eps_dual_inf = 1.0E-4; + m_settings->warm_start = true; + m_settings->max_iter = 4000; + m_settings->verbose = false; + m_settings->polish = polish; + } + m_exitflag = 0; +} + +OSQPInterface::OSQPInterface( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, const bool enable_warm_start, + const c_float eps_abs) +: OSQPInterface(enable_warm_start, eps_abs) +{ + initializeProblem(P, A, q, l, u); +} + +OSQPInterface::OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const bool enable_warm_start, + const c_float eps_abs) +: OSQPInterface(enable_warm_start, eps_abs) +{ + initializeCSCProblemImpl(P, A, q, l, u); +} + +OSQPInterface::~OSQPInterface() +{ + if (m_data->P) free(m_data->P); + if (m_data->A) free(m_data->A); +} + +void OSQPInterface::initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + initializeCSCProblemImpl(P_csc, A_csc, q, l, u); +} + +void OSQPInterface::initializeCSCProblemImpl( + CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + // Dynamic float arrays + std::vector q_tmp(q.begin(), q.end()); + std::vector l_tmp(l.begin(), l.end()); + std::vector u_tmp(u.begin(), u.end()); + double * q_dyn = q_tmp.data(); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + + /********************** + * OBJECTIVE FUNCTION + **********************/ + m_param_n = static_cast(q.size()); + m_data->m = static_cast(l.size()); + + /***************** + * POPULATE DATA + *****************/ + m_data->n = m_param_n; + if (m_data->P) free(m_data->P); + m_data->P = csc_matrix( + m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), + P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); + m_data->q = q_dyn; + if (m_data->A) free(m_data->A); + m_data->A = csc_matrix( + m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), + A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); + m_data->l = l_dyn; + m_data->u = u_dyn; + + // Setup workspace + OSQPWorkspace * workspace; + m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); + m_work.reset(workspace); + m_work_initialized = true; +} + +void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept +{ + if (ptr != nullptr) { + osqp_cleanup(ptr); + } +} + +void OSQPInterface::updateEpsAbs(const double eps_abs) +{ + m_settings->eps_abs = eps_abs; // for default setting + if (m_work_initialized) { + osqp_update_eps_abs(m_work.get(), eps_abs); // for current work + } +} + +void OSQPInterface::updateEpsRel(const double eps_rel) +{ + m_settings->eps_rel = eps_rel; // for default setting + if (m_work_initialized) { + osqp_update_eps_rel(m_work.get(), eps_rel); // for current work + } +} + +void OSQPInterface::updateMaxIter(const int max_iter) +{ + m_settings->max_iter = max_iter; // for default setting + if (m_work_initialized) { + osqp_update_max_iter(m_work.get(), max_iter); // for current work + } +} + +void OSQPInterface::updateVerbose(const bool is_verbose) +{ + m_settings->verbose = is_verbose; // for default setting + if (m_work_initialized) { + osqp_update_verbose(m_work.get(), is_verbose); // for current work + } +} + +void OSQPInterface::updateRhoInterval(const int rho_interval) +{ + m_settings->adaptive_rho_interval = rho_interval; // for default setting +} + +void OSQPInterface::updateRho(const double rho) +{ + m_settings->rho = rho; + if (m_work_initialized) { + osqp_update_rho(m_work.get(), rho); + } +} + +void OSQPInterface::updateAlpha(const double alpha) +{ + m_settings->alpha = alpha; + if (m_work_initialized) { + osqp_update_alpha(m_work.get(), alpha); + } +} + +void OSQPInterface::updateScaling(const int scaling) +{ + m_settings->scaling = scaling; +} + +void OSQPInterface::updatePolish(const bool polish) +{ + m_settings->polish = polish; + if (m_work_initialized) { + osqp_update_polish(m_work.get(), polish); + } +} + +void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) +{ + if (polish_refine_iter < 0) { + std::cerr << "Polish refinement iterations must be positive" << std::endl; + return; + } + + m_settings->polish_refine_iter = polish_refine_iter; + if (m_work_initialized) { + osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + } +} + +void OSQPInterface::updateCheckTermination(const int check_termination) +{ + if (check_termination < 0) { + std::cerr << "Check termination must be positive" << std::endl; + return; + } + + m_settings->check_termination = check_termination; + if (m_work_initialized) { + osqp_update_check_termination(m_work.get(), check_termination); + } +} + +bool OSQPInterface::setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables) +{ + return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); +} + +bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + if (result != 0) { + std::cerr << "Failed to set primal variables for warm start" << std::endl; + return false; + } + + return true; +} + +bool OSQPInterface::setDualVariables(const std::vector & dual_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + if (result != 0) { + std::cerr << "Failed to set dual variables for warm start" << std::endl; + return false; + } + + return true; +} + +void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const +{ + const int status = getStatus(); + if (status == 1) { + // No need to log since optimization was solved. + return; + } + + // create message + std::string output_message = ""; + if (prefix_message != "") { + output_message = prefix_message + " "; + } + + const auto status_message = getStatusMessage(); + output_message += "Optimization failed due to " + status_message; + + // log with warning + RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); +} + +void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) +{ + /* + // Transform 'P' into an 'upper trapezoidal matrix' + Eigen::MatrixXd P_trap = P_new.triangularView(); + // Transform 'P' into a sparse matrix and extract data as dynamic arrays + Eigen::SparseMatrix P_sparse = P_trap.sparseView(); + double *P_val_ptr = P_sparse.valuePtr(); + // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) + c_int P_elem_N = P_sparse.nonZeros(); + */ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); + osqp_update_P( + m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + +void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) +{ + osqp_update_P( + m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + +void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) +{ + /* + // Transform 'A' into a sparse matrix and extract data as dynamic arrays + Eigen::SparseMatrix A_sparse = A_new.sparseView(); + double *A_val_ptr = A_sparse.valuePtr(); + // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) + c_int A_elem_N = A_sparse.nonZeros(); + */ + CSC_Matrix A_csc = calCSCMatrix(A_new); + osqp_update_A( + m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + return; +} + +void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) +{ + osqp_update_A( + m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); +} + +void OSQPInterface::updateQ(const std::vector & q_new) +{ + std::vector q_tmp(q_new.begin(), q_new.end()); + double * q_dyn = q_tmp.data(); + osqp_update_lin_cost(m_work.get(), q_dyn); +} + +void OSQPInterface::updateL(const std::vector & l_new) +{ + std::vector l_tmp(l_new.begin(), l_new.end()); + double * l_dyn = l_tmp.data(); + osqp_update_lower_bound(m_work.get(), l_dyn); +} + +void OSQPInterface::updateU(const std::vector & u_new) +{ + std::vector u_tmp(u_new.begin(), u_new.end()); + double * u_dyn = u_tmp.data(); + osqp_update_upper_bound(m_work.get(), u_dyn); +} + +void OSQPInterface::updateBounds( + const std::vector & l_new, const std::vector & u_new) +{ + std::vector l_tmp(l_new.begin(), l_new.end()); + std::vector u_tmp(u_new.begin(), u_new.end()); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + osqp_update_bounds(m_work.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIteration() const +{ + return m_work->info->iter; +} + +int OSQPInterface::getStatus() const +{ + return static_cast(m_latest_work_info.status_val); +} + +int OSQPInterface::getPolishStatus() const +{ + return static_cast(m_latest_work_info.status_polish); +} + +std::vector OSQPInterface::getDualSolution() const +{ + double * sol_y = m_work->solution->y; + std::vector dual_solution(sol_y, sol_y + m_data->m); + return dual_solution; +} + +std::vector OSQPInterface::optimizeImpl() +{ + osqp_solve(m_work.get()); + + double * sol_x = m_work->solution->x; + double * sol_y = m_work->solution->y; + std::vector sol_primal(sol_x, sol_x + m_param_n); + std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + + m_latest_work_info = *(m_work->info); + + if (!m_enable_warm_start) { + m_work.reset(); + m_work_initialized = false; + } + + return sol_primal; +} + +std::vector OSQPInterface::optimize( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + initializeCSCProblemImpl(P, A, q, l, u); + const auto result = optimizeImpl(); + + return result; +} + +} // namespace qp diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp new file mode 100644 index 0000000000000..bf1f0229e1e21 --- /dev/null +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "qp_interface/proxqp_interface.hpp" + +namespace qp +{ +ProxQPInterface::ProxQPInterface(const bool enable_warm_start, const double eps_abs) +: QPInterface(enable_warm_start) +{ + m_settings.eps_abs = eps_abs; +} + +void ProxQPInterface::initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + const size_t variables_num = q.size(); + const size_t constraints_num = l.size(); + + const bool enable_warm_start = [&]() { + if ( + !m_enable_warm_start // Warm start is designated + || !m_qp_ptr // QP pointer is initialized + // The number of variables is the same as the previous one. + || !m_variables_num || + *m_variables_num != variables_num + // The number of constraints is the same as the previous one + || !m_constraints_num || *m_constraints_num != constraints_num) { + return false; + } + return true; + }(); + + if (!enable_warm_start) { + m_qp_ptr = std::make_shared>( + variables_num, 0, constraints_num); + } + + m_settings.initial_guess = + enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT + : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + + m_qp_ptr->settings = m_settings; + + Eigen::SparseMatrix P_sparse(variables_num, constraints_num); + P_sparse = P.sparseView(); + + // NOTE: const std vector cannot be converted to eigen vector + std::vector non_const_q = q; + Eigen::VectorXd eigen_q = + Eigen::Map(non_const_q.data(), non_const_q.size()); + std::vector l_std_vec = l; + Eigen::VectorXd eigen_l = + Eigen::Map(l_std_vec.data(), l_std_vec.size()); + std::vector u_std_vec = u; + Eigen::VectorXd eigen_u = + Eigen::Map(u_std_vec.data(), u_std_vec.size()); + + if (enable_warm_start) { + m_qp_ptr->update( + P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); + } else { + m_qp_ptr->init( + P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); + } +} + +void ProxQPInterface::updateEpsAbs(const double eps_abs) +{ + m_settings.eps_abs = eps_abs; +} + +void ProxQPInterface::updateEpsRel(const double eps_rel) +{ + m_settings.eps_rel = eps_rel; +} + +void ProxQPInterface::updateVerbose(const bool is_verbose) +{ + m_settings.verbose = is_verbose; +} + +int ProxQPInterface::getIteration() const +{ + if (m_qp_ptr) { + return m_qp_ptr->results.info.iter; + } + return 0; +} + +int ProxQPInterface::getStatus() const +{ + if (m_qp_ptr) { + return static_cast(m_qp_ptr->results.info.status); + } + return 0; +} + +std::vector ProxQPInterface::optimizeImpl() +{ + m_qp_ptr->solve(); + + std::vector result; + for (Eigen::Index i = 0; i < m_qp_ptr->results.x.size(); ++i) { + result.push_back(m_qp_ptr->results.x[i]); + } + return result; +} +} // namespace qp diff --git a/common/qp_interface/src/qp_interface.cpp b/common/qp_interface/src/qp_interface.cpp new file mode 100644 index 0000000000000..e7a69bac0c67c --- /dev/null +++ b/common/qp_interface/src/qp_interface.cpp @@ -0,0 +1,70 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "qp_interface/qp_interface.hpp" + +#include +#include +#include + +namespace qp +{ +void QPInterface::initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + // check if arguments are valid + std::stringstream ss; + if (P.rows() != P.cols()) { + ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() + << ", P.cols() = " << P.cols(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != static_cast(q.size())) { + ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() + << ", q.size() = " << q.size(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != A.cols()) { + ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() + << ", A.cols() = " << A.cols(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(l.size())) { + ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() + << ", l.size() = " << l.size(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(u.size())) { + ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() + << ", u.size() = " << u.size(); + throw std::invalid_argument(ss.str()); + } + + initializeProblemImpl(P, A, q, l, u); + + m_variables_num = q.size(); + m_constraints_num = l.size(); +} + +std::vector QPInterface::optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + initializeProblem(P, A, q, l, u); + const auto result = optimizeImpl(); + + return result; +} +} // namespace qp diff --git a/common/qp_interface/test/test_csc_matrix_conv.cpp b/common/qp_interface/test/test_csc_matrix_conv.cpp new file mode 100644 index 0000000000000..fc604d762d2c4 --- /dev/null +++ b/common/qp_interface/test/test_csc_matrix_conv.cpp @@ -0,0 +1,181 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "qp_interface/osqp_csc_matrix_conv.hpp" + +#include + +#include +#include +#include + +TEST(TestCscMatrixConv, Nominal) +{ + using qp::calCSCMatrix; + using qp::CSC_Matrix; + + Eigen::MatrixXd rect1(1, 2); + rect1 << 0.0, 1.0; + + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); + EXPECT_EQ(rect_m1.m_vals[0], 1.0); + ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); + EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); + ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); + EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); + EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + + Eigen::MatrixXd rect2(2, 4); + rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; + + const CSC_Matrix rect_m2 = calCSCMatrix(rect2); + ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); + EXPECT_EQ(rect_m2.m_vals[0], 1.0); + EXPECT_EQ(rect_m2.m_vals[1], 6.0); + EXPECT_EQ(rect_m2.m_vals[2], 3.0); + EXPECT_EQ(rect_m2.m_vals[3], 7.0); + ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); + EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); + EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); + ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); + EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); + EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); + EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + + // Example from http://netlib.org/linalg/html_templates/node92.html + Eigen::MatrixXd square2(6, 6); + square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, + 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; + + const CSC_Matrix square_m2 = calCSCMatrix(square2); + ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); + EXPECT_EQ(square_m2.m_vals[0], 10.0); + EXPECT_EQ(square_m2.m_vals[1], 3.0); + EXPECT_EQ(square_m2.m_vals[2], 3.0); + EXPECT_EQ(square_m2.m_vals[3], 9.0); + EXPECT_EQ(square_m2.m_vals[4], 7.0); + EXPECT_EQ(square_m2.m_vals[5], 8.0); + EXPECT_EQ(square_m2.m_vals[6], 4.0); + EXPECT_EQ(square_m2.m_vals[7], 8.0); + EXPECT_EQ(square_m2.m_vals[8], 8.0); + EXPECT_EQ(square_m2.m_vals[9], 7.0); + EXPECT_EQ(square_m2.m_vals[10], 7.0); + EXPECT_EQ(square_m2.m_vals[11], 9.0); + EXPECT_EQ(square_m2.m_vals[12], -2.0); + EXPECT_EQ(square_m2.m_vals[13], 5.0); + EXPECT_EQ(square_m2.m_vals[14], 9.0); + EXPECT_EQ(square_m2.m_vals[15], 2.0); + EXPECT_EQ(square_m2.m_vals[16], 3.0); + EXPECT_EQ(square_m2.m_vals[17], 13.0); + EXPECT_EQ(square_m2.m_vals[18], -1.0); + ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); + EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); + EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); + EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); + ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); + EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); + EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); + EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); + EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); + EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); +} +TEST(TestCscMatrixConv, Trapezoidal) +{ + using qp::calCSCMatrixTrapezoidal; + using qp::CSC_Matrix; + + Eigen::MatrixXd square1(2, 2); + Eigen::MatrixXd square2(3, 3); + Eigen::MatrixXd rect1(1, 2); + square1 << 1.0, 2.0, 2.0, 4.0; + square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; + rect1 << 0.0, 1.0; + + const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); + // Trapezoidal: skip the lower left triangle (2.0 in this example) + ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); + EXPECT_EQ(square_m1.m_vals[0], 1.0); + EXPECT_EQ(square_m1.m_vals[1], 2.0); + EXPECT_EQ(square_m1.m_vals[2], 4.0); + ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); + EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); + EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); + ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); + EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); + EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + + const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); + ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); + EXPECT_EQ(square_m2.m_vals[0], 2.0); + EXPECT_EQ(square_m2.m_vals[1], 5.0); + EXPECT_EQ(square_m2.m_vals[2], 6.0); + ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); + EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); + ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); + EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); + EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + + try { + const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); + FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; + } catch (const std::invalid_argument & e) { + EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); + } +} +TEST(TestCscMatrixConv, Print) +{ + using qp::calCSCMatrix; + using qp::calCSCMatrixTrapezoidal; + using qp::CSC_Matrix; + using qp::printCSCMatrix; + Eigen::MatrixXd square1(2, 2); + Eigen::MatrixXd rect1(1, 2); + square1 << 1.0, 2.0, 2.0, 4.0; + rect1 << 0.0, 1.0; + const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + printCSCMatrix(square_m1); + printCSCMatrix(rect_m1); +} diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp new file mode 100644 index 0000000000000..e5d7041469289 --- /dev/null +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -0,0 +1,169 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "qp_interface/osqp_interface.hpp" + +#include + +#include +#include + +namespace +{ +// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py +// +// min 1/2 * x' * P * x + q' * x +// s.t. lb <= A * x <= ub +// +// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] +// [1, 2] [1] [1, 0] [ 0] [0.7] +// [0, 1] [ 0] [0.7] +// [0, 1] [-inf] [inf] +// +// The optimal solution is +// x = [0.3, 0.7]' +// y = [-2.9, 0.0, 0.2, 0.0]` +// obj = 1.88 + +// cppcheck-suppress syntaxError +TEST(TestOsqpInterface, BasicQp) +{ + using qp::calCSCMatrix; + using qp::calCSCMatrixTrapezoidal; + using qp::CSC_Matrix; + + auto check_result = []( + const auto & solution, const int solution_status, const int polish_status) { + EXPECT_EQ(solution_status, 1); + EXPECT_EQ(polish_status, 1); + + static const auto ep = 1.0e-8; + + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; + + const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); + const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -qp::INF}; + const std::vector u = {1.0, 0.7, 0.7, qp::INF}; + + { + // Define problem during optimization + qp::OSQPInterface osqp(false, 1e-6); + const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + { + // Define problem during initialization + qp::OSQPInterface osqp(false, 1e-6); + const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem + Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + qp::OSQPInterface osqp(false, 1e-6); + osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); + } + + { + // Define problem during initialization with csc matrix + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + qp::OSQPInterface osqp(false, 1e-6); + + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + qp::OSQPInterface osqp(false, 1e-6); + osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + // add warm startup + { + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + qp::OSQPInterface osqp(true, 1e-6); // enable warm start + osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + { + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + + osqp.updateCheckTermination(1); + const auto primal_val = solution; + const auto dual_val = osqp.getDualSolution(); + for (size_t i = 0; i < primal_val.size(); ++i) { + std::cerr << primal_val.at(i) << std::endl; + } + osqp.setWarmStart(primal_val, dual_val); + } + + { + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + // NOTE: This should be true, but currently optimize function reset the workspace, which + // disables warm start. + // EXPECT_EQ(osqp.getTakenIter(), 1); + } +} +} // namespace diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp new file mode 100644 index 0000000000000..96466665d5172 --- /dev/null +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -0,0 +1,79 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "qp_interface/proxqp_interface.hpp" + +#include + +#include +#include + +namespace +{ +// Problem taken from +// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py +// +// min 1/2 * x' * P * x + q' * x +// s.t. lb <= A * x <= ub +// +// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] +// [1, 2] [1] [1, 0] [ 0] [0.7] +// [0, 1] [ 0] [0.7] +// [0, 1] [-inf] [inf] +// +// The optimal solution is +// x = [0.3, 0.7]' +// y = [-2.9, 0.0, 0.2, 0.0]` +// obj = 1.88 + +// cppcheck-suppress syntaxError +TEST(TestProxqpInterface, BasicQp) +{ + auto check_result = [](const auto & solution) { + static const auto ep = 1.0e-8; + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; + + const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); + const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -std::numeric_limits::max()}; + const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; + + { + // Define problem during optimization + qp::ProxQPInterface proxqp(false, 1e-9); + const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); + check_result(solution); + } + + { + // Define problem during optimization with warm start + qp::ProxQPInterface proxqp(true, 1e-9); + { + const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); + check_result(solution); + EXPECT_NE(proxqp.getIteration(), 1); + } + { + const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); + check_result(solution); + EXPECT_EQ(proxqp.getIteration(), 0); + } + } +} +} // namespace diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml index 6987e63c3c6ee..53f00386bdb4d 100644 --- a/common/rtc_manager_rviz_plugin/package.xml +++ b/common/rtc_manager_rviz_plugin/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libqt5-core libqt5-gui diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 3f6724e64f1f2..5e795ae764092 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -63,8 +63,8 @@ std::string getModuleName(const uint8_t module_type) case Module::AVOIDANCE_RIGHT: { return "avoidance_right"; } - case Module::PULL_OVER: { - return "pull_over"; + case Module::GOAL_PLANNER: { + return "goal_planner"; } case Module::PULL_OUT: { return "pull_out"; @@ -90,6 +90,9 @@ std::string getModuleName(const uint8_t module_type) case Module::OCCLUSION_SPOT: { return "occlusion_spot"; } + case Module::INTERSECTION_OCCLUSION: { + return "intersection_occlusion"; + } } return "NONE"; } @@ -102,7 +105,7 @@ bool isPathChangeModule(const uint8_t module_type) module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT || module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || - module_type == Module::PULL_OVER || module_type == Module::PULL_OUT) { + module_type == Module::GOAL_PLANNER || module_type == Module::PULL_OUT) { return true; } return false; @@ -111,7 +114,7 @@ bool isPathChangeModule(const uint8_t module_type) RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) { // TODO(tanaka): replace this magic number to Module::SIZE - const size_t module_size = 18; + const size_t module_size = 19; auto_modes_.reserve(module_size); auto * v_layout = new QVBoxLayout; auto vertical_header = new QHeaderView(Qt::Vertical); @@ -451,6 +454,7 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg } cnt++; } + rtc_table_->update(); } } // namespace rviz_plugins diff --git a/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp b/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp index 67d32a7739349..bef0a99f38403 100644 --- a/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp +++ b/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp @@ -17,6 +17,11 @@ #include +namespace signal_processing +{ +double lowpassFilter(const double current_val, const double prev_val, const double gain); +} + /** * @class First-order low-pass filter * @brief filtering values diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml index 042471134da0e..2412f916dbd15 100644 --- a/common/signal_processing/package.xml +++ b/common/signal_processing/package.xml @@ -5,15 +5,18 @@ 0.1.0 The signal processing package Takayuki Murooka + Takamasa Horibe + Fumiya Watanabe + Kyoichi Sugahara Ali Boyali Apache License 2.0 Takayuki Murooka Ali BOYALI - autoware_cmake - libboost-dev ament_cmake_auto + autoware_cmake + libboost-dev geometry_msgs rclcpp libboost-dev diff --git a/common/signal_processing/src/lowpass_filter_1d.cpp b/common/signal_processing/src/lowpass_filter_1d.cpp index 8142d907e5c2c..ce85d276d4a70 100644 --- a/common/signal_processing/src/lowpass_filter_1d.cpp +++ b/common/signal_processing/src/lowpass_filter_1d.cpp @@ -14,6 +14,14 @@ #include "signal_processing/lowpass_filter_1d.hpp" +namespace signal_processing +{ +double lowpassFilter(const double current_val, const double prev_val, const double gain) +{ + return gain * prev_val + (1.0 - gain) * current_val; +} +} // namespace signal_processing + LowpassFilter1d::LowpassFilter1d(const double gain) : gain_(gain) { } diff --git a/common/tensorrt_common/CMakeLists.txt b/common/tensorrt_common/CMakeLists.txt index 36071c9ae14f8..cb24448f1a993 100644 --- a/common/tensorrt_common/CMakeLists.txt +++ b/common/tensorrt_common/CMakeLists.txt @@ -18,6 +18,7 @@ endif() add_library(${PROJECT_NAME} SHARED src/tensorrt_common.cpp + src/simple_profiler.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/common/tensorrt_common/include/tensorrt_common/logger.hpp b/common/tensorrt_common/include/tensorrt_common/logger.hpp new file mode 100644 index 0000000000000..98fe18794998d --- /dev/null +++ b/common/tensorrt_common/include/tensorrt_common/logger.hpp @@ -0,0 +1,503 @@ +/* + * Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TENSORRT_COMMON__LOGGER_HPP_ +#define TENSORRT_COMMON__LOGGER_HPP_ + +#include "NvInferRuntimeCommon.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace tensorrt_common +{ +using Severity = nvinfer1::ILogger::Severity; + +class LogStreamConsumerBuffer : public std::stringbuf +{ +public: + LogStreamConsumerBuffer(std::ostream & stream, const std::string & prefix, bool shouldLog) + : mOutput(stream), mPrefix(prefix), mShouldLog(shouldLog) + { + } + + LogStreamConsumerBuffer(LogStreamConsumerBuffer && other) : mOutput(other.mOutput) {} + + ~LogStreamConsumerBuffer() + { + // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output + // sequence std::streambuf::pptr() gives a pointer to the current position of the output + // sequence if the pointer to the beginning is not equal to the pointer to the current position, + // call putOutput() to log the output to the stream + if (pbase() != pptr()) { + putOutput(); + } + } + + // synchronizes the stream buffer and returns 0 on success + // synchronizing the stream buffer consists of inserting the buffer contents into the stream, + // resetting the buffer and flushing the stream + virtual int sync() + { + putOutput(); + return 0; + } + + void putOutput() + { + if (mShouldLog) { + // prepend timestamp + // std::time_t timestamp = std::time(nullptr); + // tm* tm_local = std::localtime(×tamp); + mOutput << mPrefix << str(); + // set the buffer to empty + str(""); + // flush the stream + mOutput.flush(); + } + } + + void setShouldLog(bool shouldLog) { mShouldLog = shouldLog; } + +private: + std::ostream & mOutput; + std::string mPrefix; + bool mShouldLog; +}; + +//! +//! \class LogStreamConsumerBase +//! \brief Convenience object used to initialize LogStreamConsumerBuffer before std::ostream in +//! LogStreamConsumer +//! +class LogStreamConsumerBase +{ +public: + LogStreamConsumerBase(std::ostream & stream, const std::string & prefix, bool shouldLog) + : mBuffer(stream, prefix, shouldLog) + { + } + +protected: + LogStreamConsumerBuffer mBuffer; +}; + +//! +//! \class LogStreamConsumer +//! \brief Convenience object used to facilitate use of C++ stream syntax when logging messages. +//! Order of base classes is LogStreamConsumerBase and then std::ostream. +//! This is because the LogStreamConsumerBase class is used to initialize the +//! LogStreamConsumerBuffer member field in LogStreamConsumer and then the address of the buffer is +//! passed to std::ostream. This is necessary to prevent the address of an uninitialized buffer +//! from being passed to std::ostream. Please do not change the order of the parent classes. +//! +class LogStreamConsumer : protected LogStreamConsumerBase, public std::ostream +{ +public: + //! \brief Creates a LogStreamConsumer which logs messages with level severity. + //! Reportable severity determines if the messages are severe enough to be logged. + LogStreamConsumer(Severity reportableSeverity, Severity severity) + : LogStreamConsumerBase( + severityOstream(severity), severityPrefix(severity), severity <= reportableSeverity), + std::ostream(&mBuffer) // links the stream buffer with the stream + , + mShouldLog(severity <= reportableSeverity), + mSeverity(severity) + { + } + + LogStreamConsumer(LogStreamConsumer && other) + : LogStreamConsumerBase( + severityOstream(other.mSeverity), severityPrefix(other.mSeverity), other.mShouldLog), + std::ostream(&mBuffer) // links the stream buffer with the stream + , + mShouldLog(other.mShouldLog), + mSeverity(other.mSeverity) + { + } + + void setReportableSeverity(Severity reportableSeverity) + { + mShouldLog = mSeverity <= reportableSeverity; + mBuffer.setShouldLog(mShouldLog); + } + +private: + static std::ostream & severityOstream(Severity severity) + { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + static std::string severityPrefix(Severity severity) + { + switch (severity) { + case Severity::kINTERNAL_ERROR: + return "[F] "; + case Severity::kERROR: + return "[E] "; + case Severity::kWARNING: + return "[W] "; + case Severity::kINFO: + return "[I] "; + case Severity::kVERBOSE: + return "[V] "; + default: + assert(0); + return ""; + } + } + + bool mShouldLog; + Severity mSeverity; +}; + +//! \class Logger +//! +//! \brief Class which manages logging of TensorRT tools and samples +//! +//! \details This class provides a common interface for TensorRT tools and samples to log +//! information to the console, and supports logging two types of messages: +//! +//! - Debugging messages with an associated severity (info, warning, error, or internal error/fatal) +//! - Test pass/fail messages +//! +//! The advantage of having all samples use this class for logging as opposed to emitting directly +//! to stdout/stderr is that the logic for controlling the verbosity and formatting of sample output +//! is centralized in one location. +//! +//! In the future, this class could be extended to support dumping test results to a file in some +//! standard format (for example, JUnit XML), and providing additional metadata (e.g. timing the +//! duration of a test run). +//! +//! TODO: For backwards compatibility with existing samples, this class inherits directly from the +//! nvinfer1::ILogger interface, which is problematic since there isn't a clean separation between +//! messages coming from the TensorRT library and messages coming from the sample. +//! +//! In the future (once all samples are updated to use Logger::getTRTLogger() to access the ILogger) +//! we can refactor the class to eliminate the inheritance and instead make the nvinfer1::ILogger +//! implementation a member of the Logger object. + +class Logger : public nvinfer1::ILogger // NOLINT +{ +public: + // Logger(Severity severity = Severity::kWARNING) + // Logger(Severity severity = Severity::kVERBOSE) + explicit Logger(Severity severity = Severity::kINFO) : mReportableSeverity(severity) {} + + //! + //! \enum TestResult + //! \brief Represents the state of a given test + //! + enum class TestResult { + kRUNNING, //!< The test is running + kPASSED, //!< The test passed + kFAILED, //!< The test failed + kWAIVED //!< The test was waived + }; + + //! + //! \brief Forward-compatible method for retrieving the nvinfer::ILogger associated with this + //! Logger \return The nvinfer1::ILogger associated with this Logger + //! + //! TODO Once all samples are updated to use this method to register the logger with TensorRT, + //! we can eliminate the inheritance of Logger from ILogger + //! + nvinfer1::ILogger & getTRTLogger() + { + printf("verbose\n"); + return *this; + } + + //! + //! \brief Implementation of the nvinfer1::ILogger::log() virtual method + //! + //! Note samples should not be calling this function directly; it will eventually go away once we + //! eliminate the inheritance from nvinfer1::ILogger + //! + void log(Severity severity, const char * msg) noexcept override + { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + + //! + //! \brief Method for controlling the verbosity of logging output + //! + //! \param severity The logger will only emit messages that have severity of this level or higher. + //! + void setReportableSeverity(Severity severity) { mReportableSeverity = severity; } + + //! + //! \brief Opaque handle that holds logging information for a particular test + //! + //! This object is an opaque handle to information used by the Logger to print test results. + //! The sample must call Logger::defineTest() in order to obtain a TestAtom that can be used + //! with Logger::reportTest{Start,End}(). + //! + class TestAtom + { + public: + TestAtom(TestAtom &&) = default; + + private: + friend class Logger; + + TestAtom(bool started, const std::string & name, const std::string & cmdline) + : mStarted(started), mName(name), mCmdline(cmdline) + { + } + + bool mStarted; + std::string mName; + std::string mCmdline; + }; + + //! + //! \brief Define a test for logging + //! + //! \param[in] name The name of the test. This should be a string starting with + //! "TensorRT" and containing dot-separated strings containing + //! the characters [A-Za-z0-9_]. + //! For example, "TensorRT.sample_googlenet" + //! \param[in] cmdline The command line used to reproduce the test + // + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + //! + static TestAtom defineTest(const std::string & name, const std::string & cmdline) + { + return TestAtom(false, name, cmdline); + } + + //! + //! \brief A convenience overloaded version of defineTest() that accepts an array of command-line + //! arguments + //! as input + //! + //! \param[in] name The name of the test + //! \param[in] argc The number of command-line arguments + //! \param[in] argv The array of command-line arguments (given as C strings) + //! + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + static TestAtom defineTest(const std::string & name, int argc, char const * const * argv) + { + auto cmdline = genCmdlineString(argc, argv); + return defineTest(name, cmdline); + } + + //! + //! \brief Report that a test has started. + //! + //! \pre reportTestStart() has not been called yet for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has started + //! + static void reportTestStart(TestAtom & testAtom) + { + reportTestResult(testAtom, TestResult::kRUNNING); + assert(!testAtom.mStarted); + testAtom.mStarted = true; + } + + //! + //! \brief Report that a test has ended. + //! + //! \pre reportTestStart() has been called for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has ended + //! \param[in] result The result of the test. Should be one of TestResult::kPASSED, + //! TestResult::kFAILED, TestResult::kWAIVED + //! + static void reportTestEnd(const TestAtom & testAtom, TestResult result) + { + assert(result != TestResult::kRUNNING); + assert(testAtom.mStarted); + reportTestResult(testAtom, result); + } + + static int reportPass(const TestAtom & testAtom) + { + reportTestEnd(testAtom, TestResult::kPASSED); + return EXIT_SUCCESS; + } + + static int reportFail(const TestAtom & testAtom) + { + reportTestEnd(testAtom, TestResult::kFAILED); + return EXIT_FAILURE; + } + + static int reportWaive(const TestAtom & testAtom) + { + reportTestEnd(testAtom, TestResult::kWAIVED); + return EXIT_SUCCESS; + } + + static int reportTest(const TestAtom & testAtom, bool pass) + { + return pass ? reportPass(testAtom) : reportFail(testAtom); + } + + Severity getReportableSeverity() const { return mReportableSeverity; } + +private: + //! + //! \brief returns an appropriate string for prefixing a log message with the given severity + //! + static const char * severityPrefix(Severity severity) + { + switch (severity) { + case Severity::kINTERNAL_ERROR: + return "[F] "; + case Severity::kERROR: + return "[E] "; + case Severity::kWARNING: + return "[W] "; + case Severity::kINFO: + return "[I] "; + case Severity::kVERBOSE: + return "[V] "; + default: + assert(0); + return ""; + } + } + + //! + //! \brief returns an appropriate string for prefixing a test result message with the given result + //! + static const char * testResultString(TestResult result) + { + switch (result) { + case TestResult::kRUNNING: + return "RUNNING"; + case TestResult::kPASSED: + return "PASSED"; + case TestResult::kFAILED: + return "FAILED"; + case TestResult::kWAIVED: + return "WAIVED"; + default: + assert(0); + return ""; + } + } + + //! + //! \brief returns an appropriate output stream (cout or cerr) to use with the given severity + //! + static std::ostream & severityOstream(Severity severity) + { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + //! + //! \brief method that implements logging test results + //! + static void reportTestResult(const TestAtom & testAtom, TestResult result) + { + severityOstream(Severity::kINFO) << "&&&& " << testResultString(result) << " " << testAtom.mName + << " # " << testAtom.mCmdline << std::endl; + } + + //! + //! \brief generate a command line string from the given (argc, argv) values + //! + static std::string genCmdlineString(int argc, char const * const * argv) + { + std::stringstream ss; + for (int i = 0; i < argc; i++) { + if (i > 0) ss << " "; + ss << argv[i]; + } + return ss.str(); + } + + Severity mReportableSeverity; +}; + +namespace +{ + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kVERBOSE +//! +//! Example usage: +//! +//! LOG_VERBOSE(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINFO +//! +//! Example usage: +//! +//! LOG_INFO(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_INFO(const Logger & logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kWARNING +//! +//! Example usage: +//! +//! LOG_WARN(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_WARN(const Logger & logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kERROR +//! +//! Example usage: +//! +//! LOG_ERROR(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_ERROR(const Logger & logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity +//! kINTERNAL_ERROR +// ("fatal" severity) +//! +//! Example usage: +//! +//! LOG_FATAL(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_FATAL(const Logger & logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); +} + +} // anonymous namespace +} // namespace tensorrt_common + +#endif // TENSORRT_COMMON__LOGGER_HPP_ diff --git a/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp b/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp new file mode 100644 index 0000000000000..31af5fc9657d9 --- /dev/null +++ b/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp @@ -0,0 +1,70 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ +#define TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ + +#include + +#include +#include +#include +#include + +namespace tensorrt_common +{ +struct LayerInfo +{ + int in_c; + int out_c; + int w; + int h; + int k; + int stride; + int groups; + nvinfer1::LayerType type; +}; + +/** + * @class Profiler + * @brief Collect per-layer profile information, assuming times are reported in the same order + */ +class SimpleProfiler : public nvinfer1::IProfiler +{ +public: + struct Record + { + float time{0}; + int count{0}; + float min_time{-1.0}; + int index; + }; + SimpleProfiler( + std::string name, + const std::vector & src_profilers = std::vector()); + + void reportLayerTime(const char * layerName, float ms) noexcept override; + + void setProfDict(nvinfer1::ILayer * layer) noexcept; + + friend std::ostream & operator<<(std::ostream & out, SimpleProfiler & value); + +private: + std::string m_name; + std::map m_profile; + int m_index; + std::map m_layer_dict; +}; +} // namespace tensorrt_common +#endif // TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ diff --git a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp index bb271f0689002..38320625b256a 100644 --- a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp +++ b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -28,6 +28,9 @@ namespace fs = ::std::filesystem; namespace fs = ::std::experimental::filesystem; #endif +#include +#include + #include #include #include @@ -35,43 +38,71 @@ namespace fs = ::std::experimental::filesystem; namespace tensorrt_common { -class Logger : public nvinfer1::ILogger // NOLINT +/** + * @struct BuildConfig + * @brief Configuration to provide fine control regarding TensorRT builder + */ +struct BuildConfig { -public: - Logger() : Logger(Severity::kINFO) {} + // type for calibration + std::string calib_type_str; + + // DLA core ID that the process uses + int dla_core_id; + + // flag for partial quantization in first layer + bool quantize_first_layer; // For partial quantization + + // flag for partial quantization in last layer + bool quantize_last_layer; // For partial quantization - explicit Logger(Severity severity) : reportable_severity_(severity) {} + // flag for per-layer profiler using IProfiler + bool profile_per_layer; - void log(Severity severity, const char * msg) noexcept override + // clip value for implicit quantization + double clip_value; // For implicit quantization + + // Supported calibration type + const std::array valid_calib_type = {"Entropy", "Legacy", "Percentile", "MinMax"}; + + BuildConfig() + : calib_type_str("MinMax"), + dla_core_id(-1), + quantize_first_layer(false), + quantize_last_layer(false), + profile_per_layer(false), + clip_value(0.0) { - // suppress messages with severity enum value greater than the reportable - if (severity > reportable_severity_) { - return; - } + } - switch (severity) { - case Severity::kINTERNAL_ERROR: - RCLCPP_ERROR_STREAM(logger_, msg); - break; - case Severity::kERROR: - RCLCPP_ERROR_STREAM(logger_, msg); - break; - case Severity::kWARNING: - RCLCPP_WARN_STREAM(logger_, msg); - break; - case Severity::kINFO: - RCLCPP_INFO_STREAM(logger_, msg); - break; - default: - RCLCPP_INFO_STREAM(logger_, msg); - break; + explicit BuildConfig( + const std::string & calib_type_str, const int dla_core_id = -1, + const bool quantize_first_layer = false, const bool quantize_last_layer = false, + const bool profile_per_layer = false, const double clip_value = 0.0) + : calib_type_str(calib_type_str), + dla_core_id(dla_core_id), + quantize_first_layer(quantize_first_layer), + quantize_last_layer(quantize_last_layer), + profile_per_layer(profile_per_layer), + clip_value(clip_value) + { + if ( + std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == + valid_calib_type.end()) { + std::stringstream message; + message << "Invalid calibration type was specified: " << calib_type_str << std::endl + << "Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]" << std::endl + << "Default calibration type will be used: MinMax" << std::endl; + std::cerr << message.str(); } } - - Severity reportable_severity_{Severity::kWARNING}; - rclcpp::Logger logger_{rclcpp::get_logger("tensorrt_common")}; }; +nvinfer1::Dims get_input_dims(const std::string & onnx_file_path); + +const std::array valid_precisions = {"fp32", "fp16", "int8"}; +bool is_valid_precision_string(const std::string & precision); + template struct InferDeleter // NOLINT { @@ -92,18 +123,56 @@ using TrtUniquePtr = std::unique_ptr>; using BatchConfig = std::array; +/** + * @class TrtCommon + * @brief TensorRT common library + */ class TrtCommon // NOLINT { public: + /** + * @brief Construct TrtCommon. + * @param[in] mode_path ONNX model_path + * @param[in] precision precision for inference + * @param[in] calibrator pointer for any type of INT8 calibrator + * @param[in] batch_config configuration for batched execution + * @param[in] max_workspace_size maximum workspace for building TensorRT engine + * @param[in] buildConfig configuration including precision, calibration method, dla, remaining + * fp16 for first layer, remaining fp16 for last layer and profiler for builder + * @param[in] plugin_paths path for custom plugin + */ TrtCommon( const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator = nullptr, + std::unique_ptr calibrator = nullptr, const BatchConfig & batch_config = {1, 1, 1}, const size_t max_workspace_size = (16 << 20), + const BuildConfig & buildConfig = BuildConfig(), const std::vector & plugin_paths = {}); + /** + * @brief Load TensorRT engine + * @param[in] engine_file_path path for a engine file + * @return flag for whether loading are succeeded or failed + */ bool loadEngine(const std::string & engine_file_path); + + /** + * @brief Output layer information including GFLOPs and parameters + * @param[in] onnx_file_path path for a onnx file + * @warning This function is based on darknet log. + */ + void printNetworkInfo(const std::string & onnx_file_path); + + /** + * @brief build TensorRT engine from ONNX + * @param[in] onnx_file_path path for a onnx file + * @param[in] output_engine_file_path path for a engine file + */ bool buildEngineFromOnnx( const std::string & onnx_file_path, const std::string & output_engine_file_path); + + /** + * @brief setup for TensorRT execution including building and loading engine + */ void setup(); bool isInitialized(); @@ -113,13 +182,25 @@ class TrtCommon // NOLINT bool setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const; bool enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed); + /** + * @brief output per-layer information + */ + void printProfiling(void); + +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 + /** + * @brief get per-layer information for trt-engine-profiler + */ + std::string getLayerInformation(nvinfer1::LayerInformationFormat format); +#endif + private: Logger logger_; fs::path model_file_path_; TrtUniquePtr runtime_; TrtUniquePtr engine_; TrtUniquePtr context_; - std::unique_ptr calibrator_; + std::unique_ptr calibrator_; nvinfer1::Dims input_dims_; nvinfer1::Dims output_dims_; @@ -127,6 +208,13 @@ class TrtCommon // NOLINT BatchConfig batch_config_; size_t max_workspace_size_; bool is_initialized_{false}; + + // profiler for per-layer + SimpleProfiler model_profiler_; + // profiler for whole model + SimpleProfiler host_profiler_; + + std::unique_ptr build_config_; }; } // namespace tensorrt_common diff --git a/common/tensorrt_common/package.xml b/common/tensorrt_common/package.xml index 915f3899badd8..7d3995f93f7fe 100644 --- a/common/tensorrt_common/package.xml +++ b/common/tensorrt_common/package.xml @@ -7,6 +7,7 @@ Taichi Higashide Daisuke Nishimatsu Daisuke Nishimatsu + Dan Umeda Manato Hirabayashi Apache License 2.0 diff --git a/common/tensorrt_common/src/simple_profiler.cpp b/common/tensorrt_common/src/simple_profiler.cpp new file mode 100644 index 0000000000000..00aa3220b7714 --- /dev/null +++ b/common/tensorrt_common/src/simple_profiler.cpp @@ -0,0 +1,134 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace tensorrt_common +{ + +SimpleProfiler::SimpleProfiler(std::string name, const std::vector & src_profilers) +: m_name(name) +{ + float total_time = 0.0; + m_index = 0; + for (const auto & src_profiler : src_profilers) { + for (const auto & rec : src_profiler.m_profile) { + auto it = m_profile.find(rec.first); + if (it == m_profile.end()) { + m_profile.insert(rec); + } else { + it->second.time += rec.second.time; + it->second.count += rec.second.count; + total_time += rec.second.time; + } + } + } +} + +void SimpleProfiler::reportLayerTime(const char * layerName, float ms) noexcept +{ + m_profile[layerName].count++; + m_profile[layerName].time += ms; + if (m_profile[layerName].min_time == -1.0) { + m_profile[layerName].min_time = ms; + m_profile[layerName].index = m_index; + m_index++; + } else if (m_profile[layerName].min_time > ms) { + m_profile[layerName].min_time = ms; + } +} + +void SimpleProfiler::setProfDict(nvinfer1::ILayer * layer) noexcept +{ + std::string name = layer->getName(); + m_layer_dict[name]; + m_layer_dict[name].type = layer->getType(); + if (layer->getType() == nvinfer1::LayerType::kCONVOLUTION) { + nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims dim_in = in->getDimensions(); + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims dim_out = out->getDimensions(); + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + int groups = conv->getNbGroups(); + int stride = s_dims.d[0]; + int kernel = k_dims.d[0]; + m_layer_dict[name].in_c = dim_in.d[1]; + m_layer_dict[name].out_c = dim_out.d[1]; + m_layer_dict[name].w = dim_in.d[3]; + m_layer_dict[name].h = dim_in.d[2]; + m_layer_dict[name].k = kernel; + ; + m_layer_dict[name].stride = stride; + m_layer_dict[name].groups = groups; + } +} + +std::ostream & operator<<(std::ostream & out, SimpleProfiler & value) +{ + out << "========== " << value.m_name << " profile ==========" << std::endl; + float totalTime = 0; + std::string layerNameStr = "Operation"; + + int maxLayerNameLength = static_cast(layerNameStr.size()); + for (const auto & elem : value.m_profile) { + totalTime += elem.second.time; + maxLayerNameLength = std::max(maxLayerNameLength, static_cast(elem.first.size())); + } + + auto old_settings = out.flags(); + auto old_precision = out.precision(); + // Output header + { + out << "index, " << std::setw(12); + out << std::setw(maxLayerNameLength) << layerNameStr << " "; + out << std::setw(12) << "Runtime" + << "%," + << " "; + out << std::setw(12) << "Invocations" + << " , "; + out << std::setw(12) << "Runtime[ms]" + << " , "; + out << std::setw(12) << "Avg Runtime[ms]" + << " ,"; + out << std::setw(12) << "Min Runtime[ms]" << std::endl; + } + int index = value.m_index; + for (int i = 0; i < index; i++) { + for (const auto & elem : value.m_profile) { + if (elem.second.index == i) { + out << i << ", "; + out << std::setw(maxLayerNameLength) << elem.first << ","; + out << std::setw(12) << std::fixed << std::setprecision(1) + << (elem.second.time * 100.0F / totalTime) << "%" + << ","; + out << std::setw(12) << elem.second.count << ","; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) + << elem.second.time / elem.second.count << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time + << std::endl; + } + } + } + out.flags(old_settings); + out.precision(old_precision); + out << "========== " << value.m_name << " total runtime = " << totalTime + << " ms ==========" << std::endl; + return out; +} +} // namespace tensorrt_common diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 9d31b7a3e870f..e926196226052 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,20 +23,87 @@ #include #include +namespace +{ +template +bool contain(const std::string & s, const T & v) +{ + return s.find(v) != std::string::npos; +} +} // anonymous namespace + namespace tensorrt_common { +nvinfer1::Dims get_input_dims(const std::string & onnx_file_path) +{ + Logger logger_; + auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); + if (!builder) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + } + + const auto explicitBatch = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + + auto network = + TrtUniquePtr(builder->createNetworkV2(explicitBatch)); + if (!network) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); + } + + auto config = TrtUniquePtr(builder->createBuilderConfig()); + if (!config) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); + } + + auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); + if (!parser->parseFromFile( + onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to parse ONNX"); + } + + const auto input = network->getInput(0); + return input->getDimensions(); +} + +bool is_valid_precision_string(const std::string & precision) +{ + if ( + std::find(valid_precisions.begin(), valid_precisions.end(), precision) == + valid_precisions.end()) { + std::stringstream message; + message << "Invalid precision was specified: " << precision << std::endl + << "Valid string is one of: ["; + for (const auto & s : valid_precisions) { + message << s << ", "; + } + message << "] (case sensitive)" << std::endl; + std::cerr << message.str(); + return false; + } else { + return true; + } +} TrtCommon::TrtCommon( const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator, - const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, + std::unique_ptr calibrator, const BatchConfig & batch_config, + const size_t max_workspace_size, const BuildConfig & build_config, const std::vector & plugin_paths) : model_file_path_(model_path), calibrator_(std::move(calibrator)), precision_(precision), batch_config_(batch_config), - max_workspace_size_(max_workspace_size) + max_workspace_size_(max_workspace_size), + model_profiler_("Model"), + host_profiler_("Host") { + // Check given precision is valid one + if (!is_valid_precision_string(precision)) { + return; + } + build_config_ = std::make_unique(build_config); + for (const auto & plugin_path : plugin_paths) { int32_t flags{RTLD_LAZY}; // cspell: ignore asan @@ -53,6 +120,9 @@ TrtCommon::TrtCommon( } } runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); + if (build_config_->dla_core_id != -1) { + runtime_->setDLACore(build_config_->dla_core_id); + } initLibNvInferPlugins(&logger_, ""); } @@ -62,18 +132,59 @@ void TrtCommon::setup() is_initialized_ = false; return; } + std::string engine_path = model_file_path_; if (model_file_path_.extension() == ".engine") { + std::cout << "Load ... " << model_file_path_ << std::endl; loadEngine(model_file_path_); } else if (model_file_path_.extension() == ".onnx") { fs::path cache_engine_path{model_file_path_}; - cache_engine_path.replace_extension("engine"); + std::string ext; + std::string calib_name = ""; + if (precision_ == "int8") { + if (build_config_->calib_type_str == "Entropy") { + calib_name = "EntropyV2-"; + } else if ( + build_config_->calib_type_str == "Legacy" || + build_config_->calib_type_str == "Percentile") { + calib_name = "Legacy-"; + } else { + calib_name = "MinMax-"; + } + } + if (build_config_->dla_core_id != -1) { + ext = "DLA" + std::to_string(build_config_->dla_core_id) + "-" + calib_name + precision_; + if (build_config_->quantize_first_layer) { + ext += "-firstFP16"; + } + if (build_config_->quantize_last_layer) { + ext += "-lastFP16"; + } + ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; + } else { + ext = calib_name + precision_; + if (build_config_->quantize_first_layer) { + ext += "-firstFP16"; + } + if (build_config_->quantize_last_layer) { + ext += "-lastFP16"; + } + ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; + } + cache_engine_path.replace_extension(ext); + + // Output Network Information + printNetworkInfo(model_file_path_); + if (fs::exists(cache_engine_path)) { + std::cout << "Loading... " << cache_engine_path << std::endl; loadEngine(cache_engine_path); } else { + std::cout << "Building... " << cache_engine_path << std::endl; logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); buildEngineFromOnnx(model_file_path_, cache_engine_path); logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); } + engine_path = cache_engine_path; } else { is_initialized_ = false; return; @@ -86,6 +197,20 @@ void TrtCommon::setup() return; } + if (build_config_->profile_per_layer) { + context_->setProfiler(&model_profiler_); + } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 + // Write profiles for trt-engine-explorer + // See: https://github.com/NVIDIA/TensorRT/tree/main/tools/experimental/trt-engine-explorer + std::string j_ext = ".json"; + fs::path json_path{engine_path}; + json_path.replace_extension(j_ext); + std::string ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); + std::ofstream os(json_path, std::ofstream::trunc); + os << ret << std::flush; +#endif + is_initialized_ = true; } @@ -100,6 +225,110 @@ bool TrtCommon::loadEngine(const std::string & engine_file_path) return true; } +void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) +{ + auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); + if (!builder) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + return; + } + + const auto explicitBatch = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + + auto network = + TrtUniquePtr(builder->createNetworkV2(explicitBatch)); + if (!network) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); + return; + } + + auto config = TrtUniquePtr(builder->createBuilderConfig()); + if (!config) { + logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); + return; + } + + if (precision_ == "fp16" || precision_ == "int8") { + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); +#else + config->setMaxWorkspaceSize(max_workspace_size_); +#endif + + auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); + if (!parser->parseFromFile( + onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { + return; + } + int num = network->getNbLayers(); + float total_gflops = 0.0; + int total_params = 0; + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto ltype = layer->getType(); + std::string name = layer->getName(); + if (build_config_->profile_per_layer) { + model_profiler_.setProfDict(layer); + } + if (ltype == nvinfer1::LayerType::kCONSTANT) { + continue; + } + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims dim_in = in->getDimensions(); + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims dim_out = out->getDimensions(); + + if (ltype == nvinfer1::LayerType::kCONVOLUTION) { + nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + int groups = conv->getNbGroups(); + int stride = s_dims.d[0]; + int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; + float gflops = (2 * num_weights) * (dim_in.d[3] / stride * dim_in.d[2] / stride / 1e9); + ; + total_gflops += gflops; + total_params += num_weights; + std::cout << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups + << ") " + << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" + << dim_in.d[1] << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" + << dim_out.d[1]; + std::cout << " weights:" << num_weights; + std::cout << " GFLOPs:" << gflops; + std::cout << std::endl; + } else if (ltype == nvinfer1::LayerType::kPOOLING) { + nvinfer1::IPoolingLayer * pool = (nvinfer1::IPoolingLayer *)layer; + auto p_type = pool->getPoolingType(); + nvinfer1::Dims dim_stride = pool->getStrideNd(); + nvinfer1::Dims dim_window = pool->getWindowSizeNd(); + + std::cout << "L" << i << " ["; + if (p_type == nvinfer1::PoolingType::kMAX) { + std::cout << "max "; + } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { + std::cout << "avg "; + } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { + std::cout << "max avg blend "; + } + float gflops = dim_in.d[1] * dim_window.d[0] / dim_stride.d[0] * dim_window.d[1] / + dim_stride.d[1] * dim_in.d[2] * dim_in.d[3] / 1e9; + total_gflops += gflops; + std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; + std::cout << " GFLOPs:" << gflops; + std::cout << std::endl; + } else if (ltype == nvinfer1::LayerType::kRESIZE) { + std::cout << "L" << i << " [resize]" << std::endl; + } + } + std::cout << "Total " << total_gflops << " GFLOPs" << std::endl; + std::cout << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; + return; +} + bool TrtCommon::buildEngineFromOnnx( const std::string & onnx_file_path, const std::string & output_engine_file_path) { @@ -125,6 +354,23 @@ bool TrtCommon::buildEngineFromOnnx( return false; } + int num_available_dla = builder->getNbDLACores(); + if (build_config_->dla_core_id != -1) { + if (num_available_dla > 0) { + std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; + } else { + std::cout << "###Warninig : " + << "No DLA is supported! ###" << std::endl; + } + config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); + config->setDLACore(build_config_->dla_core_id); +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 + config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); +#else + config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); +#endif + config->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); + } if (precision_ == "fp16" || precision_ == "int8") { config->setFlag(nvinfer1::BuilderFlag::kFP16); } @@ -137,31 +383,87 @@ bool TrtCommon::buildEngineFromOnnx( auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); if (!parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { + std::cout << "Failed to parse onnx file" << std::endl; return false; } + const int num = network->getNbLayers(); + bool first = build_config_->quantize_first_layer; + bool last = build_config_->quantize_last_layer; + // Partial Quantization + if (precision_ == "int8") { + network->getInput(0)->setDynamicRange(0, 255.0); + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto ltype = layer->getType(); + std::string name = layer->getName(); + nvinfer1::ITensor * out = layer->getOutput(0); + if (build_config_->clip_value > 0.0) { + std::cout << "Set max value for outputs : " << build_config_->clip_value << " " << name + << std::endl; + out->setDynamicRange(0.0, build_config_->clip_value); + } + + if (ltype == nvinfer1::LayerType::kCONVOLUTION) { + if (first) { + layer->setPrecision(nvinfer1::DataType::kHALF); + std::cout << "Set kHALF in " << name << std::endl; + first = false; + } + if (last) { + if ( + contain(name, "reg_preds") || contain(name, "cls_preds") || + contain(name, "obj_preds")) { + layer->setPrecision(nvinfer1::DataType::kHALF); + std::cout << "Set kHALF in " << name << std::endl; + } + } + } + } + } + const auto input = network->getInput(0); const auto input_dims = input->getDimensions(); const auto input_channel = input_dims.d[1]; const auto input_height = input_dims.d[2]; const auto input_width = input_dims.d[3]; - auto profile = builder->createOptimizationProfile(); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, - nvinfer1::Dims4{batch_config_.at(0), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, - nvinfer1::Dims4{batch_config_.at(1), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, - nvinfer1::Dims4{batch_config_.at(2), input_channel, input_height, input_width}); - config->addOptimizationProfile(profile); - + if (batch_config_.at(0) > 1 && (batch_config_.at(0) == batch_config_.at(2))) { + // Attention : below API is deprecated in TRT8.4 + builder->setMaxBatchSize(batch_config_.at(2)); + } else { + if (build_config_->profile_per_layer) { + auto profile = builder->createOptimizationProfile(); + profile->setDimensions( + network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, + nvinfer1::Dims4{batch_config_.at(0), input_channel, input_height, input_width}); + profile->setDimensions( + network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, + nvinfer1::Dims4{batch_config_.at(1), input_channel, input_height, input_width}); + profile->setDimensions( + network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, + nvinfer1::Dims4{batch_config_.at(2), input_channel, input_height, input_width}); + config->addOptimizationProfile(profile); + } + } if (precision_ == "int8" && calibrator_) { config->setFlag(nvinfer1::BuilderFlag::kINT8); +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 + config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); +#else + config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); +#endif + // QAT requires no calibrator. + // assert((calibrator != nullptr) && "Invalid calibrator for INT8 precision"); config->setInt8Calibrator(calibrator_.get()); } + if (build_config_->profile_per_layer) { +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 + config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); +#else + config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kVERBOSE); +#endif + } #if TENSORRT_VERSION_MAJOR >= 8 auto plan = @@ -208,7 +510,20 @@ bool TrtCommon::isInitialized() nvinfer1::Dims TrtCommon::getBindingDimensions(const int32_t index) const { +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + (NV_TENSOR_PATCH * 10) >= 8500 + auto const & name = engine_->getIOTensorName(index); + auto dims = context_->getTensorShape(name); + bool const has_runtime_dim = + std::any_of(dims.d, dims.d + dims.nbDims, [](int32_t dim) { return dim == -1; }); + + if (has_runtime_dim) { + return dims; + } else { + return context_->getBindingDimensions(index); + } +#else return context_->getBindingDimensions(index); +#endif } int32_t TrtCommon::getNbBindings() @@ -223,7 +538,39 @@ bool TrtCommon::setBindingDimensions(const int32_t index, const nvinfer1::Dims & bool TrtCommon::enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed) { - return context_->enqueueV2(bindings, stream, input_consumed); + if (build_config_->profile_per_layer) { + auto inference_start = std::chrono::high_resolution_clock::now(); + + bool ret = context_->enqueueV2(bindings, stream, input_consumed); + + auto inference_end = std::chrono::high_resolution_clock::now(); + host_profiler_.reportLayerTime( + "inference", + std::chrono::duration(inference_end - inference_start).count()); + return ret; + } else { + return context_->enqueueV2(bindings, stream, input_consumed); + } +} + +void TrtCommon::printProfiling() +{ + std::cout << host_profiler_; + std::cout << std::endl; + std::cout << model_profiler_; +} + +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 +std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +{ + auto runtime = std::unique_ptr(nvinfer1::createInferRuntime(logger_)); + auto inspector = std::unique_ptr(engine_->createEngineInspector()); + if (context_ != nullptr) { + inspector->setExecutionContext(&(*context_)); + } + std::string result = inspector->getEngineInformation(format); + return result; } +#endif } // namespace tensorrt_common diff --git a/common/tier4_adapi_rviz_plugin/package.xml b/common/tier4_adapi_rviz_plugin/package.xml index a45a25898384f..1645f30e43cd4 100644 --- a/common/tier4_adapi_rviz_plugin/package.xml +++ b/common/tier4_adapi_rviz_plugin/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libqt5-core libqt5-gui diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index bfbb36f9c50e4..e8f0691a992d0 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp tier4_external_api_msgs diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml index 11d263e09c789..deda078ace22e 100644 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -10,7 +10,7 @@ Dawid Moszyński ament_cmake_auto - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs geometry_msgs libqt5-core diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index 48e646c1ff476..450549f75fdec 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -38,5 +38,6 @@ #include "tier4_autoware_utils/ros/uuid_helper.hpp" #include "tier4_autoware_utils/ros/wait_for_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/transform/transforms.hpp" #endif // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp new file mode 100644 index 0000000000000..ea88ea7624216 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp @@ -0,0 +1,51 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#define TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ + +#include +#include + +#include +#include + +namespace tier4_autoware_utils +{ +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Matrix & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} + +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Affine3f & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 9e648c49e067b..0f5f3f64ee957 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs @@ -21,6 +20,7 @@ diagnostic_msgs geometry_msgs libboost-dev + pcl_ros rclcpp tf2 tf2_geometry_msgs diff --git a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp b/common/tier4_autoware_utils/test/src/transform/test_transform.cpp new file mode 100644 index 0000000000000..aae8cc22ea9fb --- /dev/null +++ b/common/tier4_autoware_utils/test/src/transform/test_transform.cpp @@ -0,0 +1,59 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tier4_autoware_utils/transform/transforms.hpp" + +#include +#include +#include + +#include + +TEST(system, transform_point_cloud) +{ + pcl::PointCloud cloud; + cloud.push_back(pcl::PointXYZI(10.055880, -42.758892, -10.636949, 4)); + cloud.push_back(pcl::PointXYZI(23.282284, -29.485722, -11.468469, 5)); + + Eigen::Matrix transform; + transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428, + 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; + + pcl::PointCloud cloud_transformed; + tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform); + + pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4); + + constexpr float float_error = 0.0001; + EXPECT_NEAR(cloud_transformed[0].x, pt1_gt.x, float_error); + EXPECT_NEAR(cloud_transformed[0].y, pt1_gt.y, float_error); + EXPECT_NEAR(cloud_transformed[0].z, pt1_gt.z, float_error); + EXPECT_EQ(cloud_transformed[0].intensity, pt1_gt.intensity); +} + +TEST(system, empty_point_cloud) +{ + pcl::PointCloud cloud; + + Eigen::Matrix transform; + transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428, + 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; + + pcl::PointCloud cloud_transformed; + + EXPECT_NO_THROW(tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); + EXPECT_NO_FATAL_FAILURE( + tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); + EXPECT_EQ(cloud_transformed.size(), 0ul); +} diff --git a/common/tier4_calibration_rviz_plugin/package.xml b/common/tier4_calibration_rviz_plugin/package.xml index 2f8b13c4b9792..f422847d8cb70 100644 --- a/common/tier4_calibration_rviz_plugin/package.xml +++ b/common/tier4_calibration_rviz_plugin/package.xml @@ -10,8 +10,7 @@ Tomoya Kimura ament_cmake_auto - - autoware_cmake + autoware_cmake libqt5-core libqt5-widgets diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml index 6b765e0654212..73562a766ce0b 100644 --- a/common/tier4_control_rviz_plugin/package.xml +++ b/common/tier4_control_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_vehicle_msgs diff --git a/common/tier4_datetime_rviz_plugin/package.xml b/common/tier4_datetime_rviz_plugin/package.xml index d3fd7dcde4e2f..6dc0c09b32a3b 100644 --- a/common/tier4_datetime_rviz_plugin/package.xml +++ b/common/tier4_datetime_rviz_plugin/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libqt5-core libqt5-gui diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml index bb1189c025555..45b73d5b9815d 100644 --- a/common/tier4_debug_rviz_plugin/package.xml +++ b/common/tier4_debug_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake libqt5-core libqt5-gui diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp index c2ff8cfca9281..962fee8a370a4 100644 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp @@ -17,8 +17,8 @@ #define EIGEN_MPL2_ONLY -#include -#include +#include +#include #include #include diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index d4a01d67a1732..b7e0952b72178 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs geometry_msgs diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index 6ef58b8e84dcf..e103db19f1d43 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake geometry_msgs libqt5-core diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index 49a859e5c7251..c4233bd6305e3 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -23,8 +23,8 @@ #include #define EIGEN_MPL2_ONLY -#include -#include +#include +#include #include #include diff --git a/common/tier4_perception_rviz_plugin/package.xml b/common/tier4_perception_rviz_plugin/package.xml index bb130ceda42e5..e6b022f730ef3 100644 --- a/common/tier4_perception_rviz_plugin/package.xml +++ b/common/tier4_perception_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake dummy_perception_publisher libqt5-core diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp index cf07eeed99b3a..49da06aa2478e 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -100,7 +100,9 @@ CarInitialPoseTool::CarInitialPoseTool() std_dev_y_->setMin(0); std_dev_z_->setMin(0); std_dev_theta_->setMin(0); - position_z_->setMin(0); + width_->setMin(0); + length_->setMin(0); + height_->setMin(0); } void CarInitialPoseTool::onInitialize() @@ -191,7 +193,9 @@ BusInitialPoseTool::BusInitialPoseTool() std_dev_y_->setMin(0); std_dev_z_->setMin(0); std_dev_theta_->setMin(0); - position_z_->setMin(0); + width_->setMin(0); + length_->setMin(0); + height_->setMin(0); } void BusInitialPoseTool::onInitialize() @@ -284,7 +288,9 @@ BikeInitialPoseTool::BikeInitialPoseTool() std_dev_y_->setMin(0); std_dev_z_->setMin(0); std_dev_theta_->setMin(0); - position_z_->setMin(0); + width_->setMin(0); + length_->setMin(0); + height_->setMin(0); label_->addOption("BICYCLE", ObjectClassification::BICYCLE); label_->addOption("MOTORCYCLE", ObjectClassification::MOTORCYCLE); } diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp index f4eb3064855ec..52b432661467a 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp @@ -94,7 +94,6 @@ PedestrianInitialPoseTool::PedestrianInitialPoseTool() std_dev_y_->setMin(0); std_dev_z_->setMin(0); std_dev_theta_->setMin(0); - position_z_->setMin(0); } void PedestrianInitialPoseTool::onInitialize() diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp index 93d14396b3561..2060dc1a456e5 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp @@ -89,7 +89,6 @@ UnknownInitialPoseTool::UnknownInitialPoseTool() std_dev_y_->setMin(0); std_dev_z_->setMin(0); std_dev_theta_->setMin(0); - position_z_->setMin(0); } void UnknownInitialPoseTool::onInitialize() diff --git a/common/tier4_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md index c42b93191edf5..8b9154422eae6 100644 --- a/common/tier4_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -42,6 +42,7 @@ This plugin displays the path, trajectory, and maximum speed. | Name | Type | Default Value | Description | | ------------------------------- | ------ | ------------- | ---------------------------- | | `property_path_view_` | bool | true | Use Path property or not | +| `property_path_width_view_` | bool | false | Use Constant Width or not | | `property_path_width_` | float | 2.0 | Width of Path property [m] | | `property_path_alpha_` | float | 1.0 | Alpha of Path property | | `property_path_color_view_` | bool | false | Use Constant Color or not | diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index d483cac062b04..7d635f0cedcad 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -54,22 +54,57 @@ void visualizeBound( bound_object->estimateVertexCount(bound.size() * 2); bound_object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // calculate normal vector of bound and widths depending on the normal vector + std::vector normal_vector_angles; + std::vector adaptive_widths; for (size_t i = 0; i < bound.size(); ++i) { - const auto & curr_p = i == bound.size() - 1 ? bound.at(i - 1) : bound.at(i); - const auto & next_p = i == bound.size() - 1 ? bound.at(i) : bound.at(i + 1); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(curr_p, next_p); - const auto x_offset = static_cast(width * 0.5 * std::sin(yaw)); - const auto y_offset = static_cast(width * 0.5 * std::cos(yaw)); + const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { + if (i == 0) { + return std::make_pair( + tier4_autoware_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); + } + if (i == bound.size() - 1) { + return std::make_pair( + tier4_autoware_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); + } + const auto & prev_p = bound.at(i - 1); + const auto & curr_p = bound.at(i); + const auto & next_p = bound.at(i + 1); + + const float curr_to_prev_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, prev_p); + const float curr_to_next_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, next_p); + const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; + + const float diff_angle = + tier4_autoware_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); + if (diff_angle == 0.0) { + return std::make_pair(normal_vector_angle, width); + } + + return std::make_pair(normal_vector_angle, width / std::sin(diff_angle)); + }(); + + normal_vector_angles.push_back(normal_vector_angle); + adaptive_widths.push_back(adaptive_width); + } + + // calculate triangle + for (size_t i = 0; i < bound.size(); ++i) { + const float normal_vector_angle = normal_vector_angles.at(i); + const float adaptive_width = adaptive_widths.at(i); + + const auto x_offset = static_cast(adaptive_width * 0.5 * std::cos(normal_vector_angle)); + const auto y_offset = static_cast(adaptive_width * 0.5 * std::sin(normal_vector_angle)); auto target_lp = bound.at(i); - target_lp.x = target_lp.x - x_offset; + target_lp.x = target_lp.x + x_offset; target_lp.y = target_lp.y + y_offset; - target_lp.z = target_lp.z + 0.5; + target_lp.z = target_lp.z; bound_object->position(target_lp.x, target_lp.y, target_lp.z); bound_object->colour(color); auto target_rp = bound.at(i); - target_rp.x = target_rp.x + x_offset; + target_rp.x = target_rp.x - x_offset; target_rp.y = target_rp.y - y_offset; - target_rp.z = target_rp.z + 0.5; + target_rp.z = target_rp.z; bound_object->position(target_rp.x, target_rp.y, target_rp.z); bound_object->colour(color); } @@ -161,9 +196,10 @@ class AutowarePathWithLaneIdDisplay ~AutowarePathWithLaneIdDisplay(); private: - void preprocessMessageDetail( + void preProcessMessageDetail() override; + void preVisualizePathFootprintDetail( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - void processMessageDetail( + void visualizePathFootprintDetail( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; @@ -179,12 +215,16 @@ class AutowarePathDisplay : public AutowarePathWithDrivableAreaDisplay { Q_OBJECT +private: + void preProcessMessageDetail() override; }; class AutowareTrajectoryDisplay : public AutowarePathBaseDisplay { Q_OBJECT +private: + void preProcessMessageDetail() override; }; } // namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 7e48c1721a9e7..55c7a9c53d72d 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -40,8 +40,8 @@ #include #define EIGEN_MPL2_ONLY -#include -#include +#include +#include #include namespace @@ -105,6 +105,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay AutowarePathBaseDisplay() : // path property_path_view_{"View Path", true, "", this}, + property_path_width_view_{"Constant Width", false, "", &property_path_view_}, property_path_width_{"Width", 2.0, "", &property_path_view_}, property_path_alpha_{"Alpha", 1.0, "", &property_path_view_}, property_path_color_view_{"Constant Color", false, "", &property_path_view_}, @@ -211,8 +212,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay protected: virtual void visualizeDrivableArea([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} - virtual void preprocessMessageDetail([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} - virtual void processMessageDetail( + virtual void preProcessMessageDetail() {} + virtual void preVisualizePathFootprintDetail( + [[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) + { + } + virtual void visualizePathFootprintDetail( [[maybe_unused]] const typename T::ConstSharedPtr msg_ptr, [[maybe_unused]] const size_t p_idx) { } @@ -231,6 +236,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay return; } + preProcessMessageDetail(); + Ogre::Vector3 position; Ogre::Quaternion orientation; if (!this->context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { @@ -293,6 +300,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay velocity_text_nodes_.resize(msg_ptr->points.size()); } + const auto info = vehicle_footprint_info_; + const float left = property_path_width_view_.getBool() ? -property_path_width_.getFloat() / 2.0 + : -info->width / 2.0; + const float right = property_path_width_view_.getBool() ? property_path_width_.getFloat() / 2.0 + : info->width / 2.0; + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); const auto & pose = tier4_autoware_utils::getPose(path_point); @@ -314,7 +327,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay Eigen::Vector3f vec_out; Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); { - vec_in << 0, (property_path_width_.getFloat() / 2.0), 0; + vec_in << 0, right, 0; Eigen::Quaternionf quat( pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); if (!isDrivingForward(msg_ptr->points, point_idx)) { @@ -328,7 +341,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay path_manual_object_->colour(color); } { - vec_in << 0, -(property_path_width_.getFloat() / 2.0), 0; + vec_in << 0, left, 0; Eigen::Quaternionf quat( pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); if (!isDrivingForward(msg_ptr->points, point_idx)) { @@ -404,9 +417,14 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); - const float offset_from_baselink = property_offset_.getFloat(); + preVisualizePathFootprintDetail(msg_ptr); - preprocessMessageDetail(msg_ptr); + const float offset_from_baselink = property_offset_.getFloat(); + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { const auto & point = msg_ptr->points.at(p_idx); @@ -417,12 +435,6 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay color = rviz_common::properties::qtToOgre(property_footprint_color_.getColor()); color.a = property_footprint_alpha_.getFloat(); - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang - offset_from_baselink; - const float bottom = -info->rear_overhang + offset_from_baselink; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - const std::array lon_offset_vec{top, top, bottom, bottom}; const std::array lat_offset_vec{left, right, right, left}; @@ -481,13 +493,29 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay } } - processMessageDetail(msg_ptr, p_idx); + visualizePathFootprintDetail(msg_ptr, p_idx); } footprint_manual_object_->end(); point_manual_object_->end(); } + void updateVehicleInfo() + { + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + const float length{property_vehicle_length_.getFloat()}; + const float width{property_vehicle_width_.getFloat()}; + const float rear_overhang{property_rear_overhang_.getFloat()}; + + vehicle_footprint_info_ = + std::make_shared(length, width, rear_overhang); + } + } + Ogre::ManualObject * path_manual_object_{nullptr}; Ogre::ManualObject * velocity_manual_object_{nullptr}; Ogre::ManualObject * footprint_manual_object_{nullptr}; @@ -496,6 +524,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay std::vector velocity_text_nodes_; rviz_common::properties::BoolProperty property_path_view_; + rviz_common::properties::BoolProperty property_path_width_view_; rviz_common::properties::FloatProperty property_path_width_; rviz_common::properties::FloatProperty property_path_alpha_; rviz_common::properties::BoolProperty property_path_color_view_; @@ -523,6 +552,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::properties::FloatProperty property_point_radius_; rviz_common::properties::FloatProperty property_point_offset_; + std::shared_ptr vehicle_info_; + private: typename T::ConstSharedPtr last_msg_ptr_; @@ -535,24 +566,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay float length, width, rear_overhang; }; - std::shared_ptr vehicle_info_; std::shared_ptr vehicle_footprint_info_; - - void updateVehicleInfo() - { - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); - } else { - const float length{property_vehicle_length_.getFloat()}; - const float width{property_vehicle_width_.getFloat()}; - const float rear_overhang{property_rear_overhang_.getFloat()}; - - vehicle_footprint_info_ = - std::make_shared(length, width, rear_overhang); - } - } }; } // namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index b4f49033c111b..5dfa3605cfcad 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs autoware_planning_msgs diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index d805d004a56a3..f1516512c8993 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -23,6 +23,23 @@ AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() { } +void AutowarePathWithLaneIdDisplay::preProcessMessageDetail() +{ + // NOTE: This doesn't work in the constructor. + // NOTE: This doesn't work in the abstract class since the class has to have a "Q_Object" type. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_ONCE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s", + e.what()); + } + } +} + AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() { for (const auto & e : lane_id_obj_ptrs_) { @@ -32,7 +49,7 @@ AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() lane_id_obj_ptrs_.shrink_to_fit(); } -void AutowarePathWithLaneIdDisplay::preprocessMessageDetail( +void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) { const size_t size = msg_ptr->points.size(); @@ -56,7 +73,7 @@ void AutowarePathWithLaneIdDisplay::preprocessMessageDetail( } } -void AutowarePathWithLaneIdDisplay::processMessageDetail( +void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) { @@ -84,6 +101,40 @@ void AutowarePathWithLaneIdDisplay::processMessageDetail( text_ptr->setVisible(false); } } + +void AutowarePathDisplay::preProcessMessageDetail() +{ + // NOTE: This doesn't work in the constructor. + // NOTE: This doesn't work in the abstract class since the class has to have a "Q_Object" type. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_ONCE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s", + e.what()); + } + } +} + +void AutowareTrajectoryDisplay::preProcessMessageDetail() +{ + // NOTE: This doesn't work in the constructor. + // NOTE: This doesn't work in the abstract class since the class has to have a "Q_Object" type. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_ONCE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s", + e.what()); + } + } +} } // namespace rviz_plugins PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display) diff --git a/common/tier4_screen_capture_rviz_plugin/package.xml b/common/tier4_screen_capture_rviz_plugin/package.xml index 2f50771996ec0..38479dc3c38ee 100644 --- a/common/tier4_screen_capture_rviz_plugin/package.xml +++ b/common/tier4_screen_capture_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libopencv-dev libqt5-core diff --git a/common/tier4_simulated_clock_rviz_plugin/package.xml b/common/tier4_simulated_clock_rviz_plugin/package.xml index d7fd09e57b0bb..d80b18a5895b0 100644 --- a/common/tier4_simulated_clock_rviz_plugin/package.xml +++ b/common/tier4_simulated_clock_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libqt5-core libqt5-gui diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 6923bc5647d82..06d57bd947af3 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -9,8 +9,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_system_msgs diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index e97c08d169042..ce21d7fefc797 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -202,6 +202,7 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: velocity_factors_table_->setCellWidget(i, 3, label); } } + velocity_factors_table_->update(); } void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg) @@ -234,8 +235,8 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::PULL_OUT: label->setText("PULL_OUT"); break; - case SteeringFactor::PULL_OVER: - label->setText("PULL_OVER"); + case SteeringFactor::GOAL_PLANNER: + label->setText("GOAL_PLANNER"); break; case SteeringFactor::EMERGENCY_OPERATION: label->setText("EMERGENCY_OPERATION"); @@ -317,6 +318,7 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: steering_factors_table_->setCellWidget(i, 5, label); } } + steering_factors_table_->update(); } } // namespace rviz_plugins diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 214c2994e957c..21d8bae8f6cff 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_perception_msgs diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index 8f8e920299578..e4fb0095b8d0a 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -360,6 +360,7 @@ void TrafficLightPublishPanel::onTimer() traffic_table_->setCellWidget(i, 3, status_label); traffic_table_->setCellWidget(i, 4, confidence_label); } + traffic_table_->update(); } void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index 15d6609248a4e..f6c131fdc99f3 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs libqt5-core diff --git a/common/time_utils/package.xml b/common/time_utils/package.xml index a248e72f125f3..00832c1fd190a 100644 --- a/common/time_utils/package.xml +++ b/common/time_utils/package.xml @@ -8,8 +8,8 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake builtin_interfaces ament_cmake_ros diff --git a/common/tvm_utility/include/tvm_utility/pipeline.hpp b/common/tvm_utility/include/tvm_utility/pipeline.hpp index fe50028aaa176..8504da193214f 100644 --- a/common/tvm_utility/include/tvm_utility/pipeline.hpp +++ b/common/tvm_utility/include/tvm_utility/pipeline.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -200,7 +201,7 @@ typedef struct typedef struct { // Network info - std::array modelzoo_version; + std::array modelzoo_version; std::string network_name; std::string network_backend; @@ -320,7 +321,7 @@ class InferenceEngineTVM : public InferenceEngine * @param[in] version_from Earliest supported model version. * @return The version status. */ - Version version_check(const std::array & version_from) const + Version version_check(const std::array & version_from) const { auto x{config_.modelzoo_version[0]}; auto y{config_.modelzoo_version[1]}; @@ -344,7 +345,7 @@ class InferenceEngineTVM : public InferenceEngine tvm::runtime::PackedFunc execute; tvm::runtime::PackedFunc get_output; // Latest supported model version. - const std::array version_up_to{2, 1, 0}; + const std::array version_up_to{2, 1, 0}; }; template < diff --git a/common/tvm_utility/package.xml b/common/tvm_utility/package.xml index 9bc64ff554a79..c4fbf1c25ac87 100644 --- a/common/tvm_utility/package.xml +++ b/common/tvm_utility/package.xml @@ -27,7 +27,7 @@ Xinyu Wang Apache License 2.0 - autoware_cmake + autoware_cmake ament_index_cpp libopenblas-dev diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index e4e7da168b239..4f2ad6c50a9b3 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -3,14 +3,13 @@ autonomous_emergency_braking 0.1.0 - Autonomous Emergency Braking package as a ROS2 node + Autonomous Emergency Braking package as a ROS 2 node Yutaka Shimizu Takamasa Horibe Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_system_msgs diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 9934ce9b020f7..275be99f418f1 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -21,7 +21,7 @@ #include "control_performance_analysis/msg/float_stamped.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include @@ -42,6 +42,7 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; +using control_performance_analysis::msg::Error; using control_performance_analysis::msg::ErrorStamped; using control_performance_analysis::msg::FloatStamped; using geometry_msgs::msg::Pose; @@ -78,7 +79,7 @@ class ControlPerformanceAnalysisCore void setOdomHistory(const Odometry & odom); void setSteeringStatus(const SteeringReport & steering); - void findCurveRefIdx(); + boost::optional findCurveRefIdx(); std::pair findClosestPrevWayPointIdx_path_direction(); double estimateCurvature(); double estimatePurePursuitCurvature(); @@ -98,8 +99,7 @@ class ControlPerformanceAnalysisCore Params p_; // Variables Received Outside - std::shared_ptr current_waypoints_ptr_; - std::shared_ptr> current_waypoints_vel_ptr_; + std::shared_ptr current_trajectory_ptr_; std::shared_ptr current_vec_pose_ptr_; std::shared_ptr> odom_history_ptr_; // velocities at k-2, k-1, k, k+1 std::shared_ptr current_control_ptr_; @@ -112,9 +112,8 @@ class ControlPerformanceAnalysisCore // Variables computed - std::unique_ptr idx_prev_wp_; // the waypoint index, vehicle - std::unique_ptr idx_curve_ref_wp_; // index of waypoint corresponds to front axle center - std::unique_ptr idx_next_wp_; // the next waypoint index, vehicle heading to + std::unique_ptr idx_prev_wp_; // the waypoint index, vehicle + std::unique_ptr idx_next_wp_; // the next waypoint index, vehicle heading to std::unique_ptr prev_target_vars_{}; std::unique_ptr prev_driving_vars_{}; std::shared_ptr interpolated_pose_ptr_; diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 955ca4e623b4a..2d5ab8cce002d 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -57,9 +57,6 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; - // Self Pose listener. - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; // subscribe to pose listener. - // Publishers rclcpp::Publisher::SharedPtr pub_error_msg_; // publish error message rclcpp::Publisher::SharedPtr @@ -78,7 +75,7 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Parameters Params param_{}; // wheelbase, control period and feedback coefficients. // State holder - std_msgs::msg::Header last_control_cmd_; + AckermannControlCommand::ConstSharedPtr last_control_cmd_; double d_control_cmd_{0}; // Subscriber Parameters diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp index 019649927b5f9..87f331c5ff2af 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp @@ -15,15 +15,17 @@ #ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ -#include -#include +#include +#include #include +#include #include #include #include +#include #include namespace control_performance_analysis @@ -41,52 +43,20 @@ inline std::vector getNormalVector(double yaw_angle) return std::vector{-sin(yaw_angle), cos(yaw_angle)}; } -inline std::vector computeLateralLongitudinalError( - const std::vector & closest_point_position, const std::vector & vehicle_position, - const double & desired_yaw_angle) +inline std::pair computeLateralLongitudinalError( + const geometry_msgs::msg::Point & closest_point_position, + const geometry_msgs::msg::Point & vehicle_position, const double & desired_yaw_angle) { // Vector to path point originating from the vehicle r - rd std::vector vector_to_path_point{ - vehicle_position[0] - closest_point_position[0], - vehicle_position[1] - closest_point_position[1]}; + vehicle_position.x - closest_point_position.x, vehicle_position.y - closest_point_position.y}; double lateral_error = -sin(desired_yaw_angle) * vector_to_path_point[0] + cos(desired_yaw_angle) * vector_to_path_point[1]; double longitudinal_error = cos(desired_yaw_angle) * vector_to_path_point[0] + sin(desired_yaw_angle) * vector_to_path_point[1]; - return std::vector{lateral_error, longitudinal_error}; -} - -inline double computeLateralError( - std::vector & closest_point_position, std::vector & vehicle_position, - double & yaw_angle) -{ - // Normal vector of vehicle direction - std::vector normal_vector = getNormalVector(yaw_angle); - - // Vector to path point originating from the vehicle - std::vector vector_to_path_point{ - closest_point_position[0] - vehicle_position[0], - closest_point_position[1] - vehicle_position[1]}; - - double lateral_error = - normal_vector[0] * vector_to_path_point[0] + normal_vector[1] * vector_to_path_point[1]; - - return lateral_error; -} - -/* - * Shortest distance between two angles. As the angles are cyclic, interpolation between to - * angles must be carried out using the distance value instead of using the end values of - * two points. - * */ -inline double angleDistance(const double & target_angle, const double & reference_angle) -{ - double diff = std::fmod(target_angle - reference_angle + M_PI_2, 2 * M_PI) - M_PI_2; - double diff_signed_correction = diff < -M_PI_2 ? diff + 2 * M_PI : diff; // Fix sign - - return -1.0 * diff_signed_correction; + return {lateral_error, longitudinal_error}; } inline geometry_msgs::msg::Quaternion createOrientationMsgFromYaw(double yaw_angle) diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 4c83864bf9187..7003b7d931fa8 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -19,9 +19,9 @@ Berkay Karaman ament_cmake_auto + autoware_cmake rosidl_default_generators - autoware_cmake builtin_interfaces autoware_auto_control_msgs @@ -40,6 +40,7 @@ tf2_eigen tf2_ros tier4_autoware_utils + tier4_debug_msgs vehicle_info_util builtin_interfaces global_parameter_loader diff --git a/control/control_performance_analysis/scripts/control_performance_plot.py b/control/control_performance_analysis/scripts/control_performance_plot.py new file mode 100644 index 0000000000000..c5e5cabd5b059 --- /dev/null +++ b/control/control_performance_analysis/scripts/control_performance_plot.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python3 + +# Copyright 2023 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import math + +from control_performance_analysis.msg import DrivingMonitorStamped +from control_performance_analysis.msg import ErrorStamped +import matplotlib.pyplot as plt +from nav_msgs.msg import Odometry +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import BoolStamped + +parser = argparse.ArgumentParser() +parser.add_argument("-i", "--interval", help="interval distance to plot") +parser.add_argument("-r", "--realtime_plot", default=True, help="Enable real-time plotting") + + +class PlotterNode(Node): + def __init__(self, realtime_plot): + super().__init__("plotter_node") + + self.realtime_plot = realtime_plot + + self.subscription_error = self.create_subscription( + ErrorStamped, "/control_performance/performance_vars", self.error_callback, 10 + ) + self.subscription_driving = self.create_subscription( + DrivingMonitorStamped, "driving_topic", self.driving_callback, 10 + ) + self.subscription_odometry = self.create_subscription( + Odometry, "/localization/kinematic_state", self.odometry_callback, 10 + ) + self.subscription_plot = self.create_subscription( + BoolStamped, "/make_plot", self.make_plot_callback, 10 + ) + self.curvature = None + self.velocity = None + self.lateral_error = None + self.pose_distance_threshold = ( + 0.2 # Define the pose distance threshold to trigger plot update + ) + self.previous_pos = None + + self.abs_curvature_arr = [] + self.abs_velocity_arr = [] + self.abs_lateral_error_arr = [] + + self.fig, self.ax = plt.subplots(3, 3, figsize=(12, 9)) + self.velocities = [ + (0.0, 3.0), + (3.0, 6.0), + (6.0, 9.0), + (9.0, 12.0), + (12.0, 15.0), + (15.0, 18.0), + (18.0, 21.0), + (21.0, 24.0), + (24.0, float("inf")), + ] + plt.pause(0.1) + + def error_callback(self, msg): + print("error_callback!") + self.lateral_error = msg.error.lateral_error + self.curvature = msg.error.curvature_estimate + + def driving_callback(self, msg): + print("driving_callback!") + + def make_plot_callback(self, msg): + if msg.data is True: + self.update_plot() + + def odometry_callback(self, msg): + print("odometry_callback!") + current_pos = msg.pose.pose.position + self.velocity = msg.twist.twist.linear.x + + if self.previous_pos is None: + self.previous_pos = current_pos + return + + if self.curvature is None: + print("waiting curvature") + return + if self.velocity is None: + print("waiting velocity") + return + if self.lateral_error is None: + print("waiting lateral_error") + return + + pose_distance = self.calculate_distance(self.previous_pos, current_pos) + print("pose_distance = ", pose_distance) + if pose_distance >= self.pose_distance_threshold: + print("update!") + self.abs_curvature_arr.append(abs(self.curvature)) + self.abs_lateral_error_arr.append(abs(self.lateral_error)) + self.abs_velocity_arr.append(abs(self.velocity)) + if self.realtime_plot is True: + self.update_plot() + self.previous_pos = current_pos + + def calculate_distance(self, pos1, pos2): + distance = math.sqrt((pos2.x - pos1.x) ** 2 + (pos2.y - pos1.y) ** 2) + return distance + + # def update_plot(self): + # if self.curvature is not None and self.velocity is not None and self.lateral_error is not None: + # self.curvature_arr.append(self.curvature) + # self.lateral_error_arr.append(self.lateral_error) + # self.velocity_arr.append(self.velocity) + # self.ax.cla() # Clear the existing plot + # self.ax.scatter(self.curvature_arr, self.velocity_arr, self.lateral_error_arr) + # plt.pause(0.001) + # else: + # print("waiting data in update_data") + + def update_plot(self): + for i, (vel_min, vel_max) in enumerate(self.velocities): + indices = [j for j, v in enumerate(self.abs_velocity_arr) if vel_min <= v <= vel_max] + + if len(indices) > 0: + ax = self.ax[i // 3, i % 3] + ax.cla() + ax.scatter( + [self.abs_curvature_arr[j] for j in indices], + [self.abs_lateral_error_arr[j] for j in indices], + ) + ax.set_xlabel("Curvature") + ax.set_ylabel("Lateral Error") + ax.set_title(f"Velocity: {vel_min} - {vel_max}") + + plt.tight_layout() + plt.pause(0.1) # Pause to allow the plot to update + + +def main(args=None): + rclpy.init() + + args = parser.parse_args(args) + realtime_plot = args.realtime_plot + + plotter_node = PlotterNode(realtime_plot) + print("spin start!") + rclpy.spin(plotter_node) + print("spin end!") + plotter_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index b4885e43db8e3..777f5dee3e470 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -14,7 +14,8 @@ #include "control_performance_analysis/control_performance_analysis_core.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/interpolation.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -50,13 +51,7 @@ ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore(Params & p) : p_{ void ControlPerformanceAnalysisCore::setCurrentWaypoints(const Trajectory & trajectory) { - current_waypoints_ptr_ = std::make_shared(); - current_waypoints_vel_ptr_ = std::make_shared>(); - - for (const auto & point : trajectory.points) { - current_waypoints_ptr_->poses.emplace_back(point.pose); - current_waypoints_vel_ptr_->emplace_back(point.longitudinal_velocity_mps); - } + current_trajectory_ptr_ = std::make_shared(trajectory); } void ControlPerformanceAnalysisCore::setOdomHistory(const Odometry & odom) @@ -90,69 +85,45 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - auto closest_idx = motion_utils::findNearestIndex( - current_waypoints_ptr_->poses, *current_vec_pose_ptr_, p_.acceptable_max_distance_to_waypoint_, - p_.acceptable_max_yaw_difference_rad_); - - // find the prev and next waypoint - - if (*closest_idx != 0 && (*closest_idx + 1) != current_waypoints_ptr_->poses.size()) { - const double dist_to_prev = std::hypot( - current_vec_pose_ptr_->position.x - - current_waypoints_ptr_->poses.at(*closest_idx - 1).position.x, - current_vec_pose_ptr_->position.y - - current_waypoints_ptr_->poses.at(*closest_idx - 1).position.y); - const double dist_to_next = std::hypot( - current_vec_pose_ptr_->position.x - - current_waypoints_ptr_->poses.at(*closest_idx + 1).position.x, - current_vec_pose_ptr_->position.y - - current_waypoints_ptr_->poses.at(*closest_idx + 1).position.y); - if (dist_to_next > dist_to_prev) { - idx_prev_wp_ = std::make_unique(*closest_idx - 1); - idx_next_wp_ = std::make_unique(*closest_idx); - } else { - idx_prev_wp_ = std::make_unique(*closest_idx); - idx_next_wp_ = std::make_unique(*closest_idx + 1); - } - } else if (*closest_idx == 0) { - idx_prev_wp_ = std::make_unique(*closest_idx); - idx_next_wp_ = std::make_unique(*closest_idx + 1); - } else { - idx_prev_wp_ = std::make_unique(*closest_idx - 1); - idx_next_wp_ = std::make_unique(*closest_idx); + const auto closest_segment = motion_utils::findNearestSegmentIndex( + current_trajectory_ptr_->points, *current_vec_pose_ptr_, + p_.acceptable_max_distance_to_waypoint_, p_.acceptable_max_yaw_difference_rad_); + + if (!closest_segment) { // fail to find closest idx + return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - return (idx_prev_wp_ && idx_next_wp_) - ? std::make_pair(true, *idx_prev_wp_) - : std::make_pair(false, std::numeric_limits::quiet_NaN()); + + idx_prev_wp_ = std::make_unique(closest_segment.get()); + idx_next_wp_ = std::make_unique(closest_segment.get() + 1); + return std::make_pair(true, *idx_prev_wp_); } bool ControlPerformanceAnalysisCore::isDataReady() const { - rclcpp::Clock clock{RCL_ROS_TIME}; - if (!current_vec_pose_ptr_) { - RCLCPP_WARN_THROTTLE( - logger_, clock, 1000, "cannot get current pose into control_performance algorithm"); + const auto waiting = [this](const std::string & name) { + rclcpp::Clock clock{RCL_ROS_TIME}; + RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, clock, 1000, "waiting for %s data ...", name.c_str()); return false; + }; + + if (!current_vec_pose_ptr_) { + return waiting("current pose"); } - if (current_waypoints_ptr_->poses.empty()) { - RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "cannot get current trajectory waypoints ..."); - return false; + if (current_trajectory_ptr_->points.empty()) { + return waiting("current trajectory"); } if (odom_history_ptr_->size() < 2) { - RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for odometry data ..."); - return false; + return waiting("odometry"); } if (!current_control_ptr_) { - RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_control_cmd ..."); - return false; + return waiting("current_control_cmd"); } if (!current_vec_steering_msg_ptr_) { - RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_steering ..."); - return false; + return waiting("current_steering"); } return true; @@ -161,60 +132,32 @@ bool ControlPerformanceAnalysisCore::isDataReady() const bool ControlPerformanceAnalysisCore::calculateErrorVars() { // Check if data is ready. - if (!isDataReady() || !idx_prev_wp_) { - return false; - } - - // Get the interpolated pose - std::pair pair_pose_interp_wp_ = calculateClosestPose(); - - // Find and set the waypoint L-wheelbase meters ahead of the current waypoint. - findCurveRefIdx(); - - if ( - !pair_pose_interp_wp_.first || !interpolated_pose_ptr_ || !interpolated_velocity_ptr_ || - !interpolated_acceleration_ptr_ || !interpolated_steering_angle_ptr_) { - RCLCPP_WARN_THROTTLE( - logger_, clock_, 1000, - "Cannot get interpolated pose, velocity, acceleration, and steering into control_performance " - "algorithm"); + if (!isDataReady()) { return false; } - const auto pose_interp_wp_ = pair_pose_interp_wp_.second; - - // Create interpolated waypoint vector - const std::vector interp_waypoint_xy{ - pose_interp_wp_.position.x, pose_interp_wp_.position.y}; - - // Create vehicle position vector - const std::vector vehicle_position_xy{ - current_vec_pose_ptr_->position.x, current_vec_pose_ptr_->position.y}; + // update closest index + findClosestPrevWayPointIdx_path_direction(); - // Get Yaw angles of the reference waypoint and the vehicle - const double & target_yaw = tf2::getYaw(pose_interp_wp_.orientation); - const double & vehicle_yaw_angle = tf2::getYaw(current_vec_pose_ptr_->orientation); - - // Compute Curvature at the point where the front axle might follow - // get the waypoint corresponds to the front_axle center + // Get the interpolated pose + const auto [success, pose_interp_wp] = calculateClosestPose(); - if (!idx_curve_ref_wp_) { - RCLCPP_WARN(logger_, "Cannot find index of curvature reference waypoint "); + if (!success) { + RCLCPP_WARN_THROTTLE(logger_, clock_, 1000, "Cannot get interpolated variables."); return false; } - const double & curvature_est = estimateCurvature(); // three point curvature - const double & curvature_est_pp = estimatePurePursuitCurvature(); // pure pursuit curvature + const double curvature_est = estimateCurvature(); // three point curvature + const double curvature_est_pp = estimatePurePursuitCurvature(); // pure pursuit curvature // Compute lateral, longitudinal, heading error w.r.t. frenet frame - - std::vector lateral_longitudinal_error = - utils::computeLateralLongitudinalError(interp_waypoint_xy, vehicle_position_xy, target_yaw); - const double & lateral_error = lateral_longitudinal_error[0]; - const double & longitudinal_error = lateral_longitudinal_error[1]; + const double target_yaw = tf2::getYaw(pose_interp_wp.orientation); + const auto [lateral_error, longitudinal_error] = utils::computeLateralLongitudinalError( + pose_interp_wp.position, current_vec_pose_ptr_->position, target_yaw); // Compute the yaw angle error. - const double & heading_yaw_error = utils::angleDistance(vehicle_yaw_angle, target_yaw); + const double heading_yaw_error = + tier4_autoware_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); // Set the values of ErrorMsgVars. @@ -223,44 +166,42 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() error_vars.error.heading_error = heading_yaw_error; // odom history contains k + 1, k, k - 1 ... steps. We are in kth step - const uint & odom_size = odom_history_ptr_->size(); + const uint odom_size = odom_history_ptr_->size(); error_vars.header.stamp = odom_history_ptr_->at(odom_size - 2).header.stamp; // we are in step k - const double & Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; + const double Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; + // Current acceleration calculation - const double & d_x = odom_history_ptr_->at(odom_size - 1).pose.pose.position.x - - odom_history_ptr_->at(odom_size - 2).pose.pose.position.x; - const double & d_y = odom_history_ptr_->at(odom_size - 1).pose.pose.position.y - - odom_history_ptr_->at(odom_size - 2).pose.pose.position.y; - const double & ds = std::hypot(d_x, d_y); - - const double & vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + - odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x) / - 2.0; - const double & dv = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x - - odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; - const double & dt = ds / std::max(vel_mean, p_.prevent_zero_division_value_); - const double & Ax = dv / std::max(dt, p_.prevent_zero_division_value_); // current acceleration - - const double & longitudinal_error_velocity = + const auto ds = tier4_autoware_utils::calcDistance2d( + odom_history_ptr_->at(odom_size - 1).pose.pose, odom_history_ptr_->at(odom_size - 2).pose.pose); + + const double vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + + odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x) / + 2.0; + const double dv = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x - + odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; + const double dt = ds / std::max(vel_mean, p_.prevent_zero_division_value_); + const double Ax = dv / std::max(dt, p_.prevent_zero_division_value_); // current acceleration + + const double longitudinal_error_velocity = Vx * cos(heading_yaw_error) - *interpolated_velocity_ptr_ * (1 - curvature_est * lateral_error); - const double & lateral_error_velocity = + const double lateral_error_velocity = Vx * sin(heading_yaw_error) - *interpolated_velocity_ptr_ * curvature_est * longitudinal_error; - const double & steering_cmd = current_control_ptr_->lateral.steering_tire_angle; - const double & current_steering_val = current_vec_steering_msg_ptr_->steering_tire_angle; + const double steering_cmd = current_control_ptr_->lateral.steering_tire_angle; + const double current_steering_val = current_vec_steering_msg_ptr_->steering_tire_angle; error_vars.error.control_effort_energy = contR * steering_cmd * steering_cmd; // u*R*u'; - const double & heading_velocity_error = (Vx * tan(current_steering_val)) / p_.wheelbase_ - - *this->interpolated_velocity_ptr_ * curvature_est; + const double heading_velocity_error = (Vx * tan(current_steering_val)) / p_.wheelbase_ - + *this->interpolated_velocity_ptr_ * curvature_est; - const double & lateral_acceleration_error = + const double lateral_acceleration_error = -curvature_est * *interpolated_acceleration_ptr_ * longitudinal_error - curvature_est * *interpolated_velocity_ptr_ * longitudinal_error_velocity + Vx * heading_velocity_error * cos(heading_yaw_error) + Ax * sin(heading_yaw_error); - const double & longitudinal_acceleration_error = + const double longitudinal_acceleration_error = curvature_est * *interpolated_acceleration_ptr_ * lateral_error + curvature_est * *interpolated_velocity_ptr_ * lateral_error_velocity - Vx * heading_velocity_error * sin(heading_yaw_error) + Ax * cos(heading_yaw_error) - @@ -289,50 +230,23 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() if (prev_target_vars_) { // LPF for error vars - error_vars.error.curvature_estimate = - p_.lpf_gain_ * prev_target_vars_->error.curvature_estimate + - (1 - p_.lpf_gain_) * error_vars.error.curvature_estimate; - - error_vars.error.curvature_estimate_pp = - p_.lpf_gain_ * prev_target_vars_->error.curvature_estimate_pp + - (1 - p_.lpf_gain_) * error_vars.error.curvature_estimate_pp; - - error_vars.error.lateral_error = p_.lpf_gain_ * prev_target_vars_->error.lateral_error + - (1 - p_.lpf_gain_) * error_vars.error.lateral_error; - - error_vars.error.lateral_error_velocity = - p_.lpf_gain_ * prev_target_vars_->error.lateral_error_velocity + - (1 - p_.lpf_gain_) * error_vars.error.lateral_error_velocity; - - error_vars.error.lateral_error_acceleration = - p_.lpf_gain_ * prev_target_vars_->error.lateral_error_acceleration + - (1 - p_.lpf_gain_) * error_vars.error.lateral_error_acceleration; - - error_vars.error.longitudinal_error = - p_.lpf_gain_ * prev_target_vars_->error.longitudinal_error + - (1 - p_.lpf_gain_) * error_vars.error.longitudinal_error; - - error_vars.error.longitudinal_error_velocity = - p_.lpf_gain_ * prev_target_vars_->error.longitudinal_error_velocity + - (1 - p_.lpf_gain_) * error_vars.error.longitudinal_error_velocity; - - error_vars.error.longitudinal_error_acceleration = - p_.lpf_gain_ * prev_target_vars_->error.longitudinal_error_acceleration + - (1 - p_.lpf_gain_) * error_vars.error.longitudinal_error_acceleration; - - error_vars.error.heading_error = p_.lpf_gain_ * prev_target_vars_->error.heading_error + - (1 - p_.lpf_gain_) * error_vars.error.heading_error; - - error_vars.error.heading_error_velocity = - p_.lpf_gain_ * prev_target_vars_->error.heading_error_velocity + - (1 - p_.lpf_gain_) * error_vars.error.heading_error_velocity; - - error_vars.error.control_effort_energy = - p_.lpf_gain_ * prev_target_vars_->error.control_effort_energy + - (1 - p_.lpf_gain_) * error_vars.error.control_effort_energy; - - error_vars.error.error_energy = p_.lpf_gain_ * prev_target_vars_->error.error_energy + - (1 - p_.lpf_gain_) * error_vars.error.error_energy; + const auto lpf = [&](auto & var, const auto & prev_var) { + var = p_.lpf_gain_ * prev_var + (1 - p_.lpf_gain_) * var; + }; + auto & error = error_vars.error; + const auto & prev_error = prev_target_vars_->error; + lpf(error.curvature_estimate, prev_error.curvature_estimate); + lpf(error.curvature_estimate_pp, prev_error.curvature_estimate_pp); + lpf(error.lateral_error, prev_error.lateral_error); + lpf(error.lateral_error_velocity, prev_error.lateral_error_velocity); + lpf(error.lateral_error_acceleration, prev_error.lateral_error_acceleration); + lpf(error.longitudinal_error, prev_error.longitudinal_error); + lpf(error.longitudinal_error_velocity, prev_error.longitudinal_error_velocity); + lpf(error.longitudinal_error_acceleration, prev_error.longitudinal_error_acceleration); + lpf(error.heading_error, prev_error.heading_error); + lpf(error.heading_error_velocity, prev_error.heading_error_velocity); + lpf(error.control_effort_energy, prev_error.control_effort_energy); + lpf(error.error_energy, prev_error.error_energy); } prev_target_vars_ = std::make_unique(error_vars); @@ -407,30 +321,17 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() } if (prev_driving_vars_) { // LPF for driving status vars - - driving_status_vars.longitudinal_acceleration.data = - p_.lpf_gain_ * prev_driving_vars_->longitudinal_acceleration.data + - (1 - p_.lpf_gain_) * driving_status_vars.longitudinal_acceleration.data; - - driving_status_vars.lateral_acceleration.data = - p_.lpf_gain_ * prev_driving_vars_->lateral_acceleration.data + - (1 - p_.lpf_gain_) * driving_status_vars.lateral_acceleration.data; - - driving_status_vars.lateral_jerk.data = - p_.lpf_gain_ * prev_driving_vars_->lateral_jerk.data + - (1 - p_.lpf_gain_) * driving_status_vars.lateral_jerk.data; - - driving_status_vars.longitudinal_jerk.data = - p_.lpf_gain_ * prev_driving_vars_->longitudinal_jerk.data + - (1 - p_.lpf_gain_) * driving_status_vars.longitudinal_jerk.data; - - driving_status_vars.controller_processing_time.data = - p_.lpf_gain_ * prev_driving_vars_->controller_processing_time.data + - (1 - p_.lpf_gain_) * driving_status_vars.controller_processing_time.data; - - driving_status_vars.desired_steering_angle.data = - p_.lpf_gain_ * prev_driving_vars_->desired_steering_angle.data + - (1 - p_.lpf_gain_) * driving_status_vars.desired_steering_angle.data; + const auto lpf = [&](auto & var, const auto & prev_var) { + var = p_.lpf_gain_ * prev_var + (1 - p_.lpf_gain_) * var; + }; + auto & curr = driving_status_vars; + const auto & prev = prev_driving_vars_; + lpf(curr.longitudinal_acceleration.data, prev->longitudinal_acceleration.data); + lpf(curr.lateral_acceleration.data, prev->lateral_acceleration.data); + lpf(curr.lateral_jerk.data, prev->lateral_jerk.data); + lpf(curr.longitudinal_jerk.data, prev->longitudinal_jerk.data); + lpf(curr.controller_processing_time.data, prev->controller_processing_time.data); + lpf(curr.desired_steering_angle.data, prev->desired_steering_angle.data); } prev_driving_vars_ = @@ -440,8 +341,7 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() last_steering_report.stamp = current_vec_steering_msg_ptr_->stamp; } else if (last_steering_report.stamp != current_vec_steering_msg_ptr_->stamp) { - driving_status_vars.lateral_acceleration.header.set__stamp( - current_vec_steering_msg_ptr_->stamp); + driving_status_vars.lateral_acceleration.header.stamp = current_vec_steering_msg_ptr_->stamp; driving_status_vars.lateral_acceleration.data = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x * tan(current_vec_steering_msg_ptr_->steering_tire_angle) / p_.wheelbase_; @@ -460,177 +360,50 @@ void ControlPerformanceAnalysisCore::setSteeringStatus(const SteeringReport & st current_vec_steering_msg_ptr_ = std::make_shared(steering); } -void ControlPerformanceAnalysisCore::findCurveRefIdx() +boost::optional ControlPerformanceAnalysisCore::findCurveRefIdx() { // Get the previous waypoint as the reference if (!interpolated_pose_ptr_) { RCLCPP_WARN_THROTTLE( logger_, clock_, 1000, "Cannot set the curvature_idx, no valid interpolated pose ..."); - - return; + return boost::none; } - auto fun_distance_cond = [this](auto pose_t) { - const double dist = std::hypot( - pose_t.position.x - this->interpolated_pose_ptr_->position.x, - pose_t.position.y - this->interpolated_pose_ptr_->position.y); - + auto fun_distance_cond = [this](auto point_t) { + const double dist = tier4_autoware_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); return dist > p_.wheelbase_; }; auto it = std::find_if( - current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), - fun_distance_cond); + current_trajectory_ptr_->points.cbegin() + *idx_prev_wp_, + current_trajectory_ptr_->points.cend(), fun_distance_cond); - if (it == current_waypoints_ptr_->poses.cend()) { + if (it == current_trajectory_ptr_->points.cend()) { it = std::prev(it); } - const int32_t & temp_idx_curve_ref_wp = std::distance(current_waypoints_ptr_->poses.cbegin(), it); - idx_curve_ref_wp_ = std::make_unique(temp_idx_curve_ref_wp); + const auto idx_curve_ref_wp = std::distance(current_trajectory_ptr_->points.cbegin(), it); + return idx_curve_ref_wp; } std::pair ControlPerformanceAnalysisCore::calculateClosestPose() { - Pose temp_interpolated_pose; - - // Get index of prev waypoint and sanity check. - if (!idx_prev_wp_ && !idx_next_wp_) { - RCLCPP_ERROR(logger_, "Cannot find the previous and next waypoints."); - return std::make_pair(false, temp_interpolated_pose); - } - - // Define the next waypoint - so that we can define an interval in which the car follow a line. - double next_wp_acc = 0.0; - double prev_wp_acc = 0.0; - - /* - * Create two vectors originating from the previous waypoints to the next waypoint and the - * vehicle position and find projection of vehicle vector on the the trajectory section, - * - * */ - - // First get te yaw angles of all three poses. - const double & prev_yaw = - tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_prev_wp_).orientation); - const double & prev_velocity = current_waypoints_vel_ptr_->at(*idx_prev_wp_); - const double & next_velocity = current_waypoints_vel_ptr_->at(*idx_next_wp_); - - // Previous waypoint to next waypoint. - const double & dx_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; - - const double & dy_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; - - const double & delta_psi_prev2next = tf2::getYaw( - current_waypoints_ptr_->poses.at(*idx_next_wp_).orientation - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).orientation); - const double & d_vel_prev2next = next_velocity - prev_velocity; - - // Create a vector from p0 (prev) --> p1 (to next wp) - const std::vector v_prev2next_wp{dx_prev2next, dy_prev2next}; - - // Previous waypoint to the vehicle pose - /* - * p0:previous waypoint ----> p1 next waypoint - * vector = p1 - p0. We project vehicle vector on this interval to - * - * */ - - const double & dx_prev2vehicle = - current_vec_pose_ptr_->position.x - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; - - const double & dy_prev2vehicle = - current_vec_pose_ptr_->position.y - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; - - // Vector from p0 to p_vehicle - const std::vector v_prev2vehicle{dx_prev2vehicle, dy_prev2vehicle}; - - // Compute the length of v_prev2next_wp : vector from p0 --> p1 - const double & distance_p02p1 = std::hypot(dx_prev2next, dy_prev2next); - - // Compute how far the car is away from p0 in p1 direction. p_interp is the location of the - // interpolated waypoint. This is the dot product normalized by the length of the interval. - // a.b = |a|.|b|.cos(alpha) -- > |a|.cos(alpha) = a.b / |b| where b is the path interval, - - const double & distance_p02p_interp = - (dx_prev2next * dx_prev2vehicle + dy_prev2next * dy_prev2vehicle) / distance_p02p1; - - // const double & distance_p_interp2p1 = distance_p02p1 - distance_p02p_interp; - /* - * We use the following linear interpolation - * pi = p0 + ratio_t * (p1 - p0) - * */ - - const double & ratio_t = distance_p02p_interp / distance_p02p1; - - // Interpolate pose.position and pose.orientation - temp_interpolated_pose.position.x = - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x + ratio_t * dx_prev2next; - - temp_interpolated_pose.position.y = - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y + ratio_t * dy_prev2next; + const auto interp_point = + motion_utils::calcInterpolatedPoint(*current_trajectory_ptr_, *current_vec_pose_ptr_); - temp_interpolated_pose.position.z = 0.0; - - // Interpolate the yaw angle of pi : interpolated waypoint - const double & interp_yaw_angle = prev_yaw + ratio_t * delta_psi_prev2next; - const double & interp_velocity = prev_velocity + ratio_t * d_vel_prev2next; - - const Quaternion & orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); - temp_interpolated_pose.orientation = orient_msg; + const double interp_steering_angle = std::atan(p_.wheelbase_ * estimateCurvature()); - /* interpolated acceleration calculation */ + setInterpolatedVars( + interp_point.pose, interp_point.longitudinal_velocity_mps, interp_point.acceleration_mps2, + interp_steering_angle); if ( - *idx_prev_wp_ == 0 || - static_cast(*idx_prev_wp_ + 1) == current_waypoints_ptr_->poses.size()) { - prev_wp_acc = 0.0; - } else { - const double & d_x = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x - - current_waypoints_ptr_->poses.at(*idx_prev_wp_ - 1).position.x; - const double & d_y = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - - current_waypoints_ptr_->poses.at(*idx_prev_wp_ - 1).position.y; - const double & ds = std::hypot(d_x, d_y); - const double & vel_mean = (current_waypoints_vel_ptr_->at(*idx_next_wp_) + - current_waypoints_vel_ptr_->at(*idx_prev_wp_ - 1)) / - 2.0; - const double & dv = current_waypoints_vel_ptr_->at(*idx_next_wp_) - - current_waypoints_vel_ptr_->at(*idx_prev_wp_ - 1); - const double & dt = ds / std::max(vel_mean, p_.prevent_zero_division_value_); - prev_wp_acc = dv / std::max(dt, p_.prevent_zero_division_value_); + !interpolated_pose_ptr_ || !interpolated_velocity_ptr_ || !interpolated_acceleration_ptr_ || + !interpolated_steering_angle_ptr_) { + std::make_pair(false, interp_point.pose); } - if (static_cast(*idx_next_wp_ + 1) == current_waypoints_ptr_->poses.size()) { - next_wp_acc = 0.0; - } else { - const double & d_x = current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).position.x - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; - const double & d_y = current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).position.y - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; - const double & ds = std::hypot(d_x, d_y); - const double & vel_mean = (current_waypoints_vel_ptr_->at(*idx_next_wp_ + 1) + - current_waypoints_vel_ptr_->at(*idx_prev_wp_)) / - 2.0; - const double & dv = current_waypoints_vel_ptr_->at(*idx_next_wp_ + 1) - - current_waypoints_vel_ptr_->at(*idx_prev_wp_); - const double & dt = ds / std::max(vel_mean, p_.prevent_zero_division_value_); - next_wp_acc = dv / std::max(dt, p_.prevent_zero_division_value_); - } - const double & d_acc_prev2next = next_wp_acc - prev_wp_acc; - const double & interp_acceleration = prev_wp_acc + ratio_t * d_acc_prev2next; - - const Pose interpolated_pose = temp_interpolated_pose; - - /* desired steering calculation */ - - const double interp_steering_angle = std::atan(p_.wheelbase_ * estimateCurvature()); - - setInterpolatedVars( - interpolated_pose, interp_velocity, interp_acceleration, interp_steering_angle); - - return std::make_pair(true, interpolated_pose); + return std::make_pair(true, interp_point.pose); } // Sets interpolated waypoint_ptr_. @@ -646,58 +419,53 @@ void ControlPerformanceAnalysisCore::setInterpolatedVars( double ControlPerformanceAnalysisCore::estimateCurvature() { + // Find and set the waypoint L-wheelbase meters ahead of the current waypoint. + const auto idx_curve_ref_wp_optional = findCurveRefIdx(); + // Get idx of front-axle center reference point on the trajectory. // get the waypoint corresponds to the front_axle center. - if (!idx_curve_ref_wp_) { + if (!idx_curve_ref_wp_optional) { RCLCPP_WARN(logger_, "Cannot find index of curvature reference waypoint "); return 0; } + const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.get(); - const Pose front_axleWP_pose = current_waypoints_ptr_->poses.at(*idx_curve_ref_wp_); + const auto & points = current_trajectory_ptr_->points; + + const Pose front_axleWP_pose = points.at(idx_curve_ref_wp).pose; // for guarding -1 in finding previous waypoint for the front axle - const int32_t idx_prev_waypoint = - *idx_curve_ref_wp_ >= 1 ? *idx_curve_ref_wp_ - 1 : *idx_curve_ref_wp_; + const int32_t idx_prev_waypoint = std::max(idx_curve_ref_wp - 1, 0); - const Pose front_axleWP_pose_prev = current_waypoints_ptr_->poses.at(idx_prev_waypoint); + const Pose front_axleWP_pose_prev = points.at(idx_prev_waypoint).pose; // Compute arc-length ds between 2 points. - const double ds_arc_length = std::hypot( - front_axleWP_pose_prev.position.x - front_axleWP_pose.position.x, - front_axleWP_pose_prev.position.y - front_axleWP_pose.position.y); + const double ds_arc_length = + tier4_autoware_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); // Define waypoints 10 meters behind the rear axle if exist. // If not exist, we will take the first point of the // curvature triangle as the start point of the trajectory. - const auto & num_of_back_indices = std::round(p_.curvature_interval_length_ / ds_arc_length); - const int32_t loc_of_back_idx = - (*idx_curve_ref_wp_ - num_of_back_indices < 0) ? 0 : *idx_curve_ref_wp_ - num_of_back_indices; + const auto num_of_indices = + std::max(static_cast(std::round(p_.curvature_interval_length_ / ds_arc_length)), 1); + + const int32_t loc_of_back_idx = std::max(idx_curve_ref_wp - num_of_indices, 0); // Define location of forward point 10 meters ahead of the front axle on curve. - const uint32_t max_idx = - std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); + const int32_t max_idx = points.size() - 1; - const auto num_of_forward_indices = num_of_back_indices; - const int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) - ? max_idx - 1 - : *idx_curve_ref_wp_ + num_of_forward_indices - 1; + const int32_t loc_of_forward_idx = std::min(idx_curve_ref_wp + num_of_indices, max_idx); // We have three indices of the three trajectory poses. // We compute a curvature estimate from these points. - - const std::array a_coord{ - current_waypoints_ptr_->poses.at(loc_of_back_idx).position.x, - current_waypoints_ptr_->poses.at(loc_of_back_idx).position.y}; - - const std::array b_coord{ - current_waypoints_ptr_->poses.at(*idx_curve_ref_wp_).position.x, - current_waypoints_ptr_->poses.at(*idx_curve_ref_wp_).position.y}; - - const std::array c_coord{ - current_waypoints_ptr_->poses.at(loc_of_forward_idx).position.x, - current_waypoints_ptr_->poses.at(loc_of_forward_idx).position.y}; - - const double estimated_curvature = utils::curvatureFromThreePoints(a_coord, b_coord, c_coord); + double estimated_curvature = 0.0; + try { + estimated_curvature = tier4_autoware_utils::calcCurvature( + points.at(loc_of_back_idx).pose.position, points.at(idx_curve_ref_wp).pose.position, + points.at(loc_of_forward_idx).pose.position); + } catch (...) { + estimated_curvature = 0.0; + } return estimated_curvature; } @@ -712,30 +480,27 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() return 0; } - const uint32_t & odom_size = odom_history_ptr_->size(); - const double & Vx = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x; + const uint32_t odom_size = odom_history_ptr_->size(); + const double Vx = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x; const double look_ahead_distance_pp = std::max(p_.wheelbase_, 2 * Vx); - auto fun_distance_cond = [this, &look_ahead_distance_pp](auto pose_t) { - const double dist = std::hypot( - pose_t.position.x - this->interpolated_pose_ptr_->position.x, - pose_t.position.y - this->interpolated_pose_ptr_->position.y); - + auto fun_distance_cond = [this, &look_ahead_distance_pp](auto point_t) { + const double dist = tier4_autoware_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); return dist > look_ahead_distance_pp; }; auto it = std::find_if( - current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), - fun_distance_cond); + current_trajectory_ptr_->points.cbegin() + *idx_prev_wp_, + current_trajectory_ptr_->points.cend(), fun_distance_cond); Pose target_pose_pp; // If there is no waypoint left on the trajectory, interpolate one. - if (it == current_waypoints_ptr_->poses.cend()) { + if (it == current_trajectory_ptr_->points.cend()) { // Interpolate a waypoint. it = std::prev(it); - int32_t && temp_idx_pp = std::distance(current_waypoints_ptr_->poses.cbegin(), it); - Pose const & last_pose_on_traj = current_waypoints_ptr_->poses.at(temp_idx_pp); + int32_t && temp_idx_pp = std::distance(current_trajectory_ptr_->points.cbegin(), it); + const Pose & last_pose_on_traj = current_trajectory_ptr_->points.at(temp_idx_pp).pose; // get the yaw angle of the last traj point. const double & yaw_pp = tf2::getYaw(last_pose_on_traj.orientation); @@ -752,13 +517,10 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() target_pose_pp.orientation = last_pose_on_traj.orientation; } else { // idx of the last waypoint on the trajectory is - const int32_t & temp_idx_pp = std::distance(current_waypoints_ptr_->poses.cbegin(), it); - const Pose & last_pose_on_traj = current_waypoints_ptr_->poses.at(temp_idx_pp); + const int32_t temp_idx_pp = std::distance(current_trajectory_ptr_->points.cbegin(), it); + const Pose last_pose_on_traj = current_trajectory_ptr_->points.at(temp_idx_pp).pose; - target_pose_pp.position.z = last_pose_on_traj.position.z; - target_pose_pp.position.x = last_pose_on_traj.position.x; - target_pose_pp.position.y = last_pose_on_traj.position.y; - target_pose_pp.orientation = last_pose_on_traj.orientation; + target_pose_pp = last_pose_on_traj; } // We have target pose for the pure pursuit. diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index a24b4d459c93f..ad845c86fc19a 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -76,9 +76,6 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( pub_error_msg_ = create_publisher("~/output/error_stamped", 1); pub_driving_msg_ = create_publisher("~/output/driving_status_stamped", 1); - - // Wait for first self pose - self_pose_listener_.waitForFirstPose(); } void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedPtr msg) @@ -99,69 +96,41 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP void ControlPerformanceAnalysisNode::onControlRaw( const AckermannControlCommand::ConstSharedPtr control_msg) { - static bool initialized = false; - if (!control_msg) { - RCLCPP_ERROR(get_logger(), "control command has not been received yet ..."); - - return; - } else if (!initialized) { - initialized = true; - current_control_msg_ptr_ = control_msg; - last_control_cmd_.stamp = current_control_msg_ptr_->stamp; - - } else { - current_control_msg_ptr_ = control_msg; + if (last_control_cmd_) { const rclcpp::Duration & duration = - (rclcpp::Time(current_control_msg_ptr_->stamp) - rclcpp::Time(last_control_cmd_.stamp)); + (rclcpp::Time(control_msg->stamp) - rclcpp::Time(last_control_cmd_->stamp)); d_control_cmd_ = duration.seconds() * 1000; // ms - last_control_cmd_.stamp = current_control_msg_ptr_->stamp; } + + last_control_cmd_ = current_control_msg_ptr_; + current_control_msg_ptr_ = control_msg; } void ControlPerformanceAnalysisNode::onVecSteeringMeasured( const SteeringReport::ConstSharedPtr meas_steer_msg) { - if (!meas_steer_msg) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting for vehicle measured steering message ..."); - return; - } current_vec_steering_msg_ptr_ = meas_steer_msg; } void ControlPerformanceAnalysisNode::onVelocity(const Odometry::ConstSharedPtr msg) { + current_odom_ptr_ = msg; + // Sent previous state to error calculation because we need to calculate current acceleration. + control_performance_core_ptr_->setOdomHistory(*current_odom_ptr_); // k+1, k, k-1 - if (!current_odom_ptr_) { - if (isDataReady()) { - current_odom_ptr_ = msg; - current_pose_ = self_pose_listener_.getCurrentPose(); - control_performance_core_ptr_->setOdomHistory(*current_odom_ptr_); - prev_traj = *current_trajectory_ptr_; - prev_cmd = *current_control_msg_ptr_; - prev_steering = *current_vec_steering_msg_ptr_; - } + if (!isDataReady()) { return; } - control_performance_core_ptr_->setCurrentWaypoints(prev_traj); - control_performance_core_ptr_->setCurrentPose(current_pose_->pose); - control_performance_core_ptr_->setCurrentControlValue(prev_cmd); - control_performance_core_ptr_->setOdomHistory(*msg); // k+1, k, k-1 - control_performance_core_ptr_->setSteeringStatus(prev_steering); + control_performance_core_ptr_->setCurrentWaypoints(*current_trajectory_ptr_); + control_performance_core_ptr_->setCurrentPose(current_odom_ptr_->pose.pose); + control_performance_core_ptr_->setCurrentControlValue(*current_control_msg_ptr_); + control_performance_core_ptr_->setSteeringStatus(*current_vec_steering_msg_ptr_); if (!control_performance_core_ptr_->isDataReady()) { return; } - // Find the index of the next waypoint. - const std::pair & prev_closest_wp_pose_idx = - control_performance_core_ptr_->findClosestPrevWayPointIdx_path_direction(); - - if (!prev_closest_wp_pose_idx.first) { - RCLCPP_ERROR(get_logger(), "Cannot find closest waypoint"); - return; - } // Compute control performance values. if (control_performance_core_ptr_->calculateErrorVars()) { @@ -170,19 +139,13 @@ void ControlPerformanceAnalysisNode::onVelocity(const Odometry::ConstSharedPtr m RCLCPP_ERROR(get_logger(), "Cannot compute error vars ..."); } if (control_performance_core_ptr_->calculateDrivingVars()) { - control_performance_core_ptr_->driving_status_vars.controller_processing_time.header.stamp = - current_control_msg_ptr_->stamp; - control_performance_core_ptr_->driving_status_vars.controller_processing_time.data = - d_control_cmd_; - pub_driving_msg_->publish(control_performance_core_ptr_->driving_status_vars); + auto & status_vars = control_performance_core_ptr_->driving_status_vars; + status_vars.controller_processing_time.header.stamp = current_control_msg_ptr_->stamp; + status_vars.controller_processing_time.data = d_control_cmd_; + pub_driving_msg_->publish(status_vars); } else { RCLCPP_ERROR(get_logger(), "Cannot compute driving vars ..."); } - prev_traj = *current_trajectory_ptr_; - prev_cmd = *current_control_msg_ptr_; - prev_steering = *current_vec_steering_msg_ptr_; - current_odom_ptr_ = msg; - current_pose_ = self_pose_listener_.getCurrentPose(); } bool ControlPerformanceAnalysisNode::isDataReady() const @@ -199,6 +162,11 @@ bool ControlPerformanceAnalysisNode::isDataReady() const return false; } + if (!current_vec_steering_msg_ptr_) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current steering ..."); + return false; + } + return true; } diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp index 423c71fb286fc..467d26bbb69a0 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -35,18 +35,5 @@ double triangleArea( return 0.5 * (m1 + m2 + m3); } -double curvatureFromThreePoints( - std::array const & a, std::array const & b, std::array const & c) -{ - double area = triangleArea(a, b, c); - - double a_mag = std::hypot(a[0] - b[0], a[1] - b[1]); // magnitude of triangle edges - double b_mag = std::hypot(b[0] - c[0], b[1] - c[1]); - double c_mag = std::hypot(c[0] - a[0], c[1] - a[1]); - - double curvature = 4 * area / std::max(a_mag * b_mag * c_mag, 1e-4); - - return curvature; -} } // namespace utils } // namespace control_performance_analysis diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml index f6ca2e165b08c..1b27c9f07915a 100644 --- a/control/external_cmd_selector/package.xml +++ b/control/external_cmd_selector/package.xml @@ -17,8 +17,7 @@ Kenji Miyake ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs diagnostic_updater diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml index 7244c2432045e..0c2defb2fc768 100644 --- a/control/joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -17,8 +17,7 @@ Kenji Miyake ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_vehicle_msgs diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 0ade872d2ae08..617c391f2f284 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -10,8 +10,7 @@ Kenji Miyake ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_planning_msgs diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index b1b67e03f2ab8..73603ec62ad8b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -15,12 +15,13 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include + #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #else diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index a189ecc2ea7c8..b73d43ec68e59 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -15,9 +15,9 @@ #ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Dense" -#include "eigen3/Eigen/LU" +#include +#include +#include namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 8b7158610b385..5258dc5f3f89e 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -15,11 +15,12 @@ #ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#include "eigen3/Eigen/Dense" #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" +#include + namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp index 0130cf6c4a279..692ae0c14a439 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp @@ -15,11 +15,12 @@ #ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ #define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Dense" -#include "eigen3/Eigen/LU" #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include +#include +#include + #include namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index b167efe28d810..c35e39de67b45 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -47,10 +47,11 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include +#include + namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 244ded818a1a2..ea843afaf7271 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -41,10 +41,11 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include +#include + namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 133c4df251fea..1dafb1e0bb652 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -38,10 +38,11 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include +#include + namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 36a5be08b29f8..1bef540460a64 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -15,7 +15,7 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "eigen3/Eigen/Core" +#include namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index 04a80ef9d9080..da03c4481e782 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -15,8 +15,7 @@ Takayuki Murooka ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_planning_msgs diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 9bd4d4ab02769..dba803e8f5efc 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -60,8 +60,14 @@ bool QPSolverOSQP::solve( const int status_val = std::get<3>(result); if (status_val != 1) { - // TODO(Horibe): Should return false and the failure must be handled in an appropriate way. RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(U_osqp.begin(), U_osqp.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; } // polish status: successful (1), unperformed (0), (-1) unsuccessful diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index e7a51abdac598..8a202ac689229 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -18,8 +18,7 @@ Satoshi Tanaka ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs boost diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 86dc23b6c0368..dbe9a21c1186a 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -8,7 +8,7 @@ Takamasa Horibe - autoware_cmake + autoware_cmake rosidl_default_generators autoware_auto_control_msgs diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 7147a76b8eeff..418b3f7d27220 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -68,6 +68,13 @@ There are two sources of the slope information, which can be switched by a param - Cons: z-coordinates of high-precision map is needed. - Cons: Does not support free space planning (for now) +**Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. + +This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. +For instance, if the vehicle is attempting to start with an acceleration of `1.0 m/s^2` and a gravity correction of `-1.0 m/s^2` is applied, the output value will be `0`. If this output value is mistakenly treated as the target acceleration, the vehicle will not start. + +A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. + #### PID control For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index e918ee9836c9c..5584e1e79e971 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -15,12 +15,12 @@ #ifndef PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" +#include +#include #include // NOLINT #include "autoware_auto_planning_msgs/msg/trajectory.hpp" diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 4112171633af9..c2dbc67f011dc 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -16,8 +16,6 @@ #define PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ #include "diagnostic_updater/diagnostic_updater.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" #include "pid_longitudinal_controller/debug_values.hpp" #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "pid_longitudinal_controller/lowpass_filter.hpp" @@ -30,6 +28,9 @@ #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index e8825eaab1778..0c2da18f9185c 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -15,8 +15,7 @@ Takayuki Murooka ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_control_msgs diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index b435e5e4f589f..78e47edb2cf48 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -526,6 +526,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d RCLCPP_INFO_SKIPFIRST_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", s); }; + // if current operation mode is not autonomous mode, then change state to stopped + if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) { + return changeState(ControlState::STOPPED); + } + // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index d2b3ed292c7c6..ef0810c695ea1 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -12,8 +12,8 @@ Takamasa Horibe ament_cmake_auto + autoware_cmake - autoware_cmake autoware_auto_control_msgs autoware_auto_planning_msgs boost diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index 02619bb2b11b7..d260376e55aa2 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include @@ -32,6 +33,7 @@ class ShiftDecider : public rclcpp::Node void onTimer(); void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); + void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg); void updateCurrentShiftCmd(); void initTimer(double period_s); @@ -40,11 +42,15 @@ class ShiftDecider : public rclcpp::Node sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_current_gear_; + rclcpp::TimerBase::SharedPtr timer_; autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; + autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; + bool park_on_goal_; }; diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index c5bcfccb0ad43..b24dbab1786e1 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -10,8 +10,7 @@ Takamasa Horibe ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_system_msgs diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 5eb3ce7cd0f36..3fcd45d8e51d1 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -38,6 +38,8 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); sub_autoware_state_ = create_subscription( "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); + sub_current_gear_ = create_subscription( + "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); initTimer(0.1); } @@ -53,9 +55,14 @@ void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState autoware_state_ = msg; } +void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) +{ + current_gear_ptr_ = msg; +} + void ShiftDecider::onTimer() { - if (!autoware_state_ || !control_cmd_) { + if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { return; } @@ -70,12 +77,20 @@ void ShiftDecider::updateCurrentShiftCmd() shift_cmd_.stamp = now(); static constexpr double vel_threshold = 0.01; // to prevent chattering - if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) { - shift_cmd_.command = GearCommand::PARK; - } else if (control_cmd_->longitudinal.speed > vel_threshold) { - shift_cmd_.command = GearCommand::DRIVE; - } else if (control_cmd_->longitudinal.speed < -vel_threshold) { - shift_cmd_.command = GearCommand::REVERSE; + if (autoware_state_->state == AutowareState::DRIVING) { + if (control_cmd_->longitudinal.speed > vel_threshold) { + shift_cmd_.command = GearCommand::DRIVE; + } else if (control_cmd_->longitudinal.speed < -vel_threshold) { + shift_cmd_.command = GearCommand::REVERSE; + } else { + shift_cmd_.command = current_gear_ptr_->report; + } + } else { + if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) { + shift_cmd_.command = GearCommand::PARK; + } else { + shift_cmd_.command = current_gear_ptr_->report; + } } } diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index b3e6a53228fbb..d896f874a3a20 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -17,8 +17,7 @@ Takayuki Murooka ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_control_msgs diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 6fc7a0cbedc59..4868fa7a6f51d 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -15,8 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ #define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" @@ -26,6 +24,9 @@ #include "trajectory_follower_node/visibility_control.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 303df922a2958..8673634e24058 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -17,8 +17,7 @@ Takayuki Murooka ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_control_msgs diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 48e7288150b4e..644e937e0718d 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -26,7 +26,6 @@ - diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index 92d6911c8f69d..f2a3fc83e8d63 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -11,8 +11,7 @@ Hiroki OTA ament_cmake - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_control_msgs diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/vehicle_cmd_gate/script/delays_checker.py new file mode 100644 index 0000000000000..564b7c7ac94e5 --- /dev/null +++ b/control/vehicle_cmd_gate/script/delays_checker.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_vehicle_msgs.msg import Engage +from geometry_msgs.msg import AccelWithCovarianceStamped +from nav_msgs.msg import Odometry +import rclpy +from rclpy.node import Node + +accel_topic = "/localization/acceleration" +odom_topic = "/localization/kinematic_state" +in_gate_cmd_topic = "/control/trajectory_follower/control_cmd" +out_gate_cmd_topic = "/control/command/control_cmd" +engage_topic = "/autoware/engage" + + +# This node measure the delays between when we want ego to move, and when it actually starts moving. +# There are 2 cases measured: +# 1) After engaging +# Before autoware is engaged, assuming a valid and non stopping trajectory is generated +# by the planning module, +# then the controller should output a control command with positive speed and acceleration. +# In that case, the delay measured is the duration between autoware being engaged, +# and the vehicle starting to move (based on the measured velocity). +# 2) Restart +# When already engaged, the ego vehicle may stop and restart multiple times. +# (e.g., when stopping at a red light). In that case, +# the trajectory is stopping and the control command has a non-positive velocity and acceleration. +# In that case, the delay measured is the duration between the controller's command switching to +# a positive velocity and acceleration, and the ego vehicle actually starting to move. +class DelaysChecker(Node): + def __init__(self): + super().__init__("delays_checker") + + self.autoware_engage = None + self.ego_is_stopped = True + self.prev_in_cmd = None + self.last_engage_time = None + self.last_start_time = None + self.current_accel = 0.0 + + # planning path and trajectories + self.sub_accel = self.create_subscription( + AccelWithCovarianceStamped, + accel_topic, + self.CallBackAccel, + 1, + ) + self.sub_odom = self.create_subscription( + Odometry, + odom_topic, + self.CallBackOdom, + 1, + ) + self.sub_engage = self.create_subscription(Engage, engage_topic, self.CallBackEngage, 1) + self.sub_in_gate_cmd = self.create_subscription( + AckermannControlCommand, + in_gate_cmd_topic, + self.CallBackInCmd, + 1, + ) + self.sub_out_gate_cmd = self.create_subscription( + AckermannControlCommand, + out_gate_cmd_topic, + self.CallBackOutCmd, + 1, + ) + + def CallBackEngage(self, msg): + if not self.autoware_engage or msg.engage != self.autoware_engage: + self.autoware_engage = msg.engage + if self.autoware_engage == 1: + self.last_engage_time = self.get_clock().now() + + def CallBackInCmd(self, msg): + is_start = ( + self.prev_in_cmd + and self.ego_is_stopped + and self.prev_in_cmd.longitudinal.acceleration < 0.0 + and msg.longitudinal.acceleration > 0.0 + ) + if is_start: + self.last_start_time = self.get_clock().now() + self.prev_in_cmd = msg + + def CallBackOutCmd(self, msg): + None + + def CallBackAccel(self, msg): + self.current_accel = msg.accel.accel.linear.x + + def CallBackOdom(self, msg): + if msg.twist.twist.linear.x < 1e-6: + if not self.ego_is_stopped: + self.last_engage_time = None + self.ego_is_stopped = True + elif self.ego_is_stopped: + # Ego starts to move + now = self.get_clock().now() + delay_ms = ( + (now - self.last_engage_time).nanoseconds * 1e-6 + if self.last_engage_time + else ( + (now - self.last_start_time).nanoseconds * 1e-6 if self.last_start_time else 0.0 + ) + ) + self.get_logger().info( + "Move delay {}ms ({})".format( + delay_ms, ("after engage" if self.last_engage_time else "restart") + ) + ) + self.ego_is_stopped = False + + +def main(args=None): + try: + rclpy.init(args=args) + node = DelaysChecker() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index b10e1828d35f3..b5609b1088530 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -88,8 +88,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); mrm_state_sub_ = create_subscription( "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); - gear_status_sub_ = create_subscription( - "input/gear_status", 1, [this](GearReport::SharedPtr msg) { current_gear_ptr_ = msg; }); // Subscriber for auto auto_control_cmd_sub_ = create_subscription( @@ -321,15 +319,6 @@ void VehicleCmdGate::onTimer() return; } - if (is_gate_mode_changed_) { - // If gate mode is external, is_engaged_ is always true - // While changing gate mode external to auto, the first is_engaged_ is always true for the first - // loop in this scope. So we need to wait for the second loop - // after gate mode is changed. - is_gate_mode_changed_ = false; - return; - } - // Select commands TurnIndicatorsCommand turn_indicator; HazardLightsCommand hazard_light; @@ -346,11 +335,6 @@ void VehicleCmdGate::onTimer() // Don't send turn signal when autoware is not engaged if (!is_engaged_) { - if (!current_gear_ptr_) { - gear.command = GearCommand::NONE; - } else { - gear.command = current_gear_ptr_.get()->report; - } turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND; hazard_light.command = HazardLightsCommand::NO_COMMAND; } @@ -425,6 +409,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Publish commands vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_commands.control); + pause_->publish(); // Save ControlCmd to steering angle when disengaged prev_control_cmd_ = filtered_commands.control; @@ -496,6 +481,9 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont AckermannControlCommand out = in; const double dt = getDt(); const auto mode = current_operation_mode_; + const auto current_status_cmd = getActualStatusAsCommand(); + const auto ego_is_stopped = std::abs(current_status_cmd.longitudinal.speed) < 1e-3; + const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; // Apply transition_filter when transiting from MANUAL to AUTO. if (mode.is_in_transition) { @@ -507,8 +495,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont // set prev value for both to keep consistency over switching: // Actual steer, vel, acc should be considered in manual mode to prevent sudden motion when // switching from manual to autonomous - auto prev_values = - (mode.mode == OperationModeState::AUTONOMOUS) ? out : getActualStatusAsCommand(); + auto prev_values = (mode.mode == OperationModeState::AUTONOMOUS) ? out : current_status_cmd; // TODO(Horibe): To prevent sudden acceleration/deceleration when switching from manual to // autonomous, the filter should be applied for actual speed and acceleration during manual @@ -520,6 +507,14 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont // filter in manual mode. prev_values.longitudinal = out.longitudinal; // TODO(Horibe): to be removed + // When ego is stopped and the input command is stopping, + // use the actual vehicle longitudinal state for the next filtering + // this is to prevent the jerk limits being applied on the "stop acceleration" + // which may be negative and cause delays when restarting the vehicle. + if (ego_is_stopped && input_cmd_is_stopping) { + prev_values.longitudinal = current_status_cmd.longitudinal; + } + filter_.setPrevCmd(prev_values); filter_on_transition_.setPrevCmd(prev_values); @@ -566,7 +561,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) { const auto prev_gate_mode = current_gate_mode_; current_gate_mode_ = *msg; - is_gate_mode_changed_ = true; + if (current_gate_mode_.data != prev_gate_mode.data) { RCLCPP_INFO( get_logger(), "GateMode changed: %s -> %s", getGateModeName(prev_gate_mode.data), diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 19e5bfc8a9eda..ee74bbdb90acf 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -27,7 +27,6 @@ #include #include #include -#include #include #include #include @@ -52,7 +51,6 @@ using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::GearReport; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; @@ -104,7 +102,6 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Subscription::SharedPtr gate_mode_sub_; rclcpp::Subscription::SharedPtr operation_mode_sub_; rclcpp::Subscription::SharedPtr mrm_state_sub_; - rclcpp::Subscription::SharedPtr gear_status_sub_; rclcpp::Subscription::SharedPtr kinematics_sub_; // for filter rclcpp::Subscription::SharedPtr acc_sub_; // for filter rclcpp::Subscription::SharedPtr steer_sub_; // for filter @@ -116,11 +113,9 @@ class VehicleCmdGate : public rclcpp::Node bool is_engaged_; bool is_system_emergency_ = false; bool is_external_emergency_stop_ = false; - bool is_gate_mode_changed_ = false; double current_steer_ = 0; GateMode current_gate_mode_; MrmState current_mrm_state_; - GearReport::ConstSharedPtr current_gear_ptr_; Odometry current_kinematics_; double current_acceleration_ = 0.0; diff --git a/evaluator/diagnostic_converter/package.xml b/evaluator/diagnostic_converter/package.xml index c561cfcac6c65..be9266f39b6e0 100644 --- a/evaluator/diagnostic_converter/package.xml +++ b/evaluator/diagnostic_converter/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_msgs pluginlib diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml index e761a53cb6292..a83538cdb3196 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -2,15 +2,14 @@ kinematic_evaluator 0.1.0 - kinematic evaluator ROS2 node + kinematic evaluator ROS 2 node Dominik Jargot Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index 7f99d100233b1..30b2bf0980fa0 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -2,15 +2,14 @@ localization_evaluator 0.1.0 - localization evaluator ROS2 node + localization evaluator ROS 2 node Dominik Jargot Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml index ef2bca288c8b0..fb3270fb0d89b 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -3,7 +3,7 @@ planning_evaluator 0.1.0 - ROS2 node for evaluating planners + ROS 2 node for evaluating planners Maxime CLEMENT Kyoichi Sugahara Apache License 2.0 @@ -11,8 +11,7 @@ Maxime CLEMENT ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index 46ad5c79b8b4f..b9b86e6bd2d12 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -14,9 +14,10 @@ #include "planning_evaluator/metrics/obstacle_metrics.hpp" -#include "eigen3/Eigen/Core" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include + #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp index 931764f5bb669..abd3ca5584951 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp @@ -14,10 +14,11 @@ #include "planning_evaluator/metrics/stability_metrics.hpp" -#include "eigen3/Eigen/Core" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include + #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include diff --git a/launch/map4_localization_launch/CMakeLists.txt b/launch/map4_localization_launch/CMakeLists.txt new file mode 100644 index 0000000000000..0fb53c57411b1 --- /dev/null +++ b/launch/map4_localization_launch/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.14) +project(map4_localization_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE + eagleye_pose_twist_localization_launch + eagleye_twist_localization_launch +) diff --git a/launch/map4_localization_launch/README.md b/launch/map4_localization_launch/README.md new file mode 100644 index 0000000000000..76ad751dee235 --- /dev/null +++ b/launch/map4_localization_launch/README.md @@ -0,0 +1,44 @@ +# map4_localization_launch + +This localization launch package is a sample package to introduce eagleye into autoware. + +Eagleye provides a cost-effective alternative to LiDAR and point cloud-based localization by using low-cost GNSS and IMU sensors to provide vehicle position and orientation. + +Autoware users will be able to choose between their existing LiDAR and point cloud-based localization stacks or GNSS/IMU-based eagleye localizer, depending on their specific needs and operating environment. + +In addition to the LiDAR-based solution provided by `tier4_localization_launch` (shown in the first row of the following table), there are two ways to utilize eagleye in `map4_localization_launch` with the autoware localization stack. + +| localization launch  | twist estimator | pose estimator | +| --------------------------------------------------------------- | --------------------------------- | --------------------------------- | +| tier4_localization_launch | gyro_odometry  | ndt_scan_matcher | +| map4_localization_launch/eagleye_twist_localization_launch | eagleye_rt(gyro/odom/gnss fusion) | ndt_scan_matcher | +| map4_localization_launch/eagleye_pose_twist_localization_launch | eagleye_rt(gyro/odom/gnss fusion) | eagleye_rt(gyro/odom/gnss fusion) | + +## Structure + +- eagleye_twist_localization_launch + +![map4_localization_launch/eagleye_twist_localization_launch](./eagleye_twist_localization_launch/localization_launch.drawio.svg) + +- eagleye_pose_twist_localization_launch + +![map4_localization_launch/eagleye_pose_twist_localization_launch](./eagleye_pose_twist_localization_launch/localization_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +Include `localization.launch.xml` in other launch files as follows. + +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `localization.launch.xml`. + +```xml + + + + + ... + +``` diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization.launch.xml b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization.launch.xml new file mode 100644 index 0000000000000..ed08d2d8dba20 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization_launch.drawio.svg b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization_launch.drawio.svg new file mode 100644 index 0000000000000..05c33a642a5d4 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization_launch.drawio.svg @@ -0,0 +1,299 @@ + + + + + + + + + + +
+
+
+ localization.launch.xml +
+
+
+ package: localization + _launch +
+
+
+
+ + localization.launch.xml... + + + + + + + + + + +
+
+
+ util.launch.xml +
+
+
package: localization_launch
+
+
+
+ + util.launch.xml... + + + + + + + + + + +
+
+
+ pose_twist_estimator.launch.xml +
+
+
+ package: localization_launch +
+
+
+
+ + pose_twist_estimator.launch.xml... + + + + + + +
+
+
+ launch name +
+
+
+ package: package name +
+
+
+
+ + launch name... + + + + + + +
+
+
+ ex: +
+
+
+ + ex: + + + + + + +
+
+
+ node name +
+
+
+ package: package name +
+
+
+
+ + node name... + + + + + + +
+
+
+ other name +
+
+
+ package: package name +
+
+
+
+ + other name... + + + + + + +
+
+
+ pose_twist_fusion_filter.launch.xml +
+
+
+ package: localization_launch +
+
+
+
+ + pose_twist_fusion_filter.launch.xml... + + + + + + + + +
+
+
+ localization_error_monitor.launch.xml +
+
+
+ package: localization_error_monitor +
+
+
+
+ + localization_error_monitor.launch.xml... + + + + + + + + +
+
+
+ eagleye_rt.launch.xml +
+
+
+ package: eagleye_rt +
+
+
+
+ + eagleye_rt.launch.xml... + + + + + + + + +
+
+
+ ekf_localizer.launch.xml +
+
+
+ package: ekf_localizer +
+
+
+
+ + ekf_localizer.launch.xml... + + + + + + + + +
+
+
+ pose_initializer.launch.xml +
+
+
+ package: pose_initializer +
+
+
+
+ + pose_initializer.launch.xml... + + + + + + + Text is not SVG - cannot display + + + diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml new file mode 100644 index 0000000000000..edb4ce64f2b1a --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/fix2pose.launch.xml b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/fix2pose.launch.xml new file mode 100644 index 0000000000000..ac92fce40cd6a --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/fix2pose.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/gnss_converter.launch.xml b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/gnss_converter.launch.xml new file mode 100644 index 0000000000000..efd7e94ada116 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/gnss_converter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/pose_twist_estimator.launch.xml new file mode 100644 index 0000000000000..23d5cbe75ead5 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/util/util.launch.xml b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/util/util.launch.xml new file mode 100644 index 0000000000000..6043d1b3dd37c --- /dev/null +++ b/launch/map4_localization_launch/eagleye_pose_twist_localization_launch/util/util.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_twist_localization_launch/localization.launch.xml b/launch/map4_localization_launch/eagleye_twist_localization_launch/localization.launch.xml new file mode 100644 index 0000000000000..871e936bf3292 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_twist_localization_launch/localization.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_twist_localization_launch/localization_launch.drawio.svg b/launch/map4_localization_launch/eagleye_twist_localization_launch/localization_launch.drawio.svg new file mode 100644 index 0000000000000..76ab922b3aad5 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_twist_localization_launch/localization_launch.drawio.svg @@ -0,0 +1,470 @@ + + + + + + + + + + +
+
+
+ /sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container +
+
+
+ package: rclcpp_components +
+
+
+
+ + /sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container... + + + + + + +
+
+
+ localization.launch.xml +
+
+
+ package: localization + _launch +
+
+
+
+ + localization.launch.xml... + + + + + + + + + + +
+
+
+ util.launch.xml +
+
+
package: localization_launch
+
+
+
+ + util.launch.xml... + + + + + + + + + + +
+
+
+ pose_estimator.launch.xml +
+
+
+ package: localization_launch +
+
+
+
+ + pose_estimator.launch.xml... + + + + + + +
+
+
+ launch name +
+
+
+ package: package name +
+
+
+
+ + launch name... + + + + + + +
+
+
+ ex: +
+
+
+ + ex: + + + + + + +
+
+
+ node name +
+
+
+ package: package name +
+
+
+
+ + node name... + + + + + + +
+
+
+ other name +
+
+
+ package: package name +
+
+
+
+ + other name... + + + + + + + + +
+
+
+ twist_estimator.launch.xml +
+
+
+ package: localization_launch +
+
+
+
+ + twist_estimator.launch.xml... + + + + + + + + +
+
+
+ pose_twist_fusion_filter.launch.xml +
+
+
+ package: localization_launch +
+
+
+
+ + pose_twist_fusion_filter.launch.xml... + + + + + + + + +
+
+
+ localization_error_monitor.launch.xml +
+
+
+ package: localization_error_monitor +
+
+
+
+ + localization_error_monitor.launch.xml... + + + + + + + + +
+
+
+ ndt_scan_matcher.launch.xml +
+
+
+ package: ndt_scan_matcher +
+
+
+
+ + ndt_scan_matcher.launch.xml... + + + + + + + + +
+
+
+ eagleye_rt.launch.xml +
+
+
+ package: eagleye_rt +
+
+
+
+ + eagleye_rt.launch.xml... + + + + + + + + +
+
+
+ ekf_localizer.launch.xml +
+
+
+ package: ekf_localizer +
+
+
+
+ + ekf_localizer.launch.xml... + + + + + + + + +
+
+
+ pose_initializer.launch.xml +
+
+
+ package: pose_initializer +
+
+
+
+ + pose_initializer.launch.xml... + + + + + + +
+
+
+ util.launch.py +
+
+
+ package: localization_launch +
+
+
+
+ + util.launch.py... + + + + + + + + +
+
+
+ crop_box_measurement_range +
+
+
+ package: pointcloud_preprocessor +
+
+
+
+ + crop_box_measurement_range... + + + + + + +
+
+
+ voxel_grid_downsample_filter +
+
+
+ package: pointcloud_preprocessor +
+
+
+
+ + voxel_grid_downsample_filter... + + + + + + +
+
+
+ random_downsample_filter +
+
+
+ package: pointcloud_preprocessor +
+
+
+
+ + random_downsample_filter... + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/eagleye_rt.launch.xml b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/eagleye_rt.launch.xml new file mode 100644 index 0000000000000..859508ec1a802 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/eagleye_rt.launch.xml @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/fix2pose.launch.xml b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/fix2pose.launch.xml new file mode 100644 index 0000000000000..ac92fce40cd6a --- /dev/null +++ b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/fix2pose.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/gnss_converter.launch.xml b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/gnss_converter.launch.xml new file mode 100644 index 0000000000000..efd7e94ada116 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/eagleye/gnss_converter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/twist_estimator.launch.xml b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/twist_estimator.launch.xml new file mode 100644 index 0000000000000..b73dfb8a92686 --- /dev/null +++ b/launch/map4_localization_launch/eagleye_twist_localization_launch/twist_estimator/twist_estimator.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/map4_localization_launch/package.xml b/launch/map4_localization_launch/package.xml new file mode 100644 index 0000000000000..dc390a6a9086e --- /dev/null +++ b/launch/map4_localization_launch/package.xml @@ -0,0 +1,35 @@ + + + + map4_localization_launch + 0.1.0 + The map4_localization_launch package + Yamato Ando + Koji Minoda + Kento Yabuuchi + Ryu Yamamoto + Ryohei Sasaki + Apache License 2.0 + Ryohei Sasaki + + ament_cmake_auto + autoware_cmake + + automatic_pose_initializer + eagleye_fix2pose + eagleye_gnss_converter + eagleye_rt + ekf_localizer + gyro_odometer + ndt_scan_matcher + pointcloud_preprocessor + pose_initializer + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/launch/tier4_autoware_api_launch/README.md b/launch/tier4_autoware_api_launch/README.md index fe409160f8e51..2b3d6fc032d65 100644 --- a/launch/tier4_autoware_api_launch/README.md +++ b/launch/tier4_autoware_api_launch/README.md @@ -18,4 +18,4 @@ You can include as follows in `*.launch.xml` to use `autoware_api.launch.xml`. ## Notes -For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS 2 (similar to Nodelet in ROS 1 ) diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 55b57dbda5a73..bed0b9e58ca75 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -12,8 +12,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake ad_api_adaptors autoware_iv_external_api_adaptor diff --git a/launch/tier4_control_launch/README.md b/launch/tier4_control_launch/README.md index 2d2a6487986bd..939b45626f3c5 100644 --- a/launch/tier4_control_launch/README.md +++ b/launch/tier4_control_launch/README.md @@ -27,4 +27,4 @@ Note that you should provide parameter paths as `PACKAGE_param_path`. The list o ## Notes -For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS 2 (similar to Nodelet in ROS 1 ) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c658b29ccd30b..8cb9c726fcb05 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -121,6 +121,7 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("input/control_cmd", "/control/trajectory_follower/control_cmd"), ("input/state", "/autoware/state"), + ("input/current_gear", "/vehicle/status/gear_status"), ("output/gear_cmd", "/control/shift_decider/gear_cmd"), ], parameters=[ @@ -178,7 +179,6 @@ def launch_setup(context, *args, **kwargs): ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"), ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"), ("input/mrm_state", "/system/fail_safe/mrm_state"), - ("input/gear_status", "/vehicle/status/gear_status"), ("input/kinematics", "/localization/kinematic_state"), ("input/acceleration", "/localization/acceleration"), ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"), @@ -347,7 +347,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("check_external_emergency_heartbeat") # component - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 108e43915b26f..4bfefa4d93747 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -9,8 +9,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake external_cmd_converter external_cmd_selector diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index dd58311afe2df..ef9f25e43cf90 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -114,7 +114,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to the parameter file of random_downsample_filter", ) - add_launch_arg("use_intra_process", "true", "use ROS2 component container communication") + add_launch_arg("use_intra_process", "true", "use ROS 2 component container communication") add_launch_arg("use_pointcloud_container", "True", "use pointcloud container") add_launch_arg( "pointcloud_container_name", diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index 31a453c082522..c1e0b4641256c 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -9,13 +9,14 @@ - + + diff --git a/launch/tier4_localization_launch/localization_launch.drawio.svg b/launch/tier4_localization_launch/localization_launch.drawio.svg index 0710c72c53c62..d08ebadee96bb 100644 --- a/launch/tier4_localization_launch/localization_launch.drawio.svg +++ b/launch/tier4_localization_launch/localization_launch.drawio.svg @@ -1,25 +1,27 @@ + + + - + - +
-
-
+
+
/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container

@@ -30,16 +32,16 @@
- /sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container... + /sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container... - +
-
-
+
+
localization.launch.xml

@@ -54,50 +56,50 @@ localization.launch.xml... - - - - - + + + + + - +
-
-
+
+
util.launch.xml

-
package: tier4_localization_launch
+
package: localization_launch
- util.launch.xml... + util.launch.xml... - - - - + + + + - +
-
-
+
+
pose_estimator.launch.xml

- package: tier4_localization_launch + package: localization_launch
@@ -109,10 +111,10 @@ - +
-
-
+
+
launch name

@@ -129,26 +131,28 @@ - +
-
-
ex:
+
+
+ ex: +
- ex: + ex: - +
-
-
+
+
node name

@@ -165,13 +169,13 @@ - +
-
-
+
+
other name

@@ -185,23 +189,23 @@ other name... - - + + - +
-
-
+
+
twist_estimator.launch.xml

- package: tier4_localization_launch + package: localization_launch
@@ -210,23 +214,23 @@ twist_estimator.launch.xml... - - + + - +
-
-
+
+
pose_twist_fusion_filter.launch.xml

- package: tier4_localization_launch + package: localization_launch
@@ -235,18 +239,18 @@ pose_twist_fusion_filter.launch.xml... - - + + - +
-
-
+
+
localization_error_monitor.launch.xml

@@ -260,54 +264,18 @@ localization_error_monitor.launch.xml... - - - - - - -
-
-
- relay -
-
-
- package: topic_tools -
-
- -
- -
-
- input: /localization/pose_twist_fusion_filter/twist -
-
- output: /localization/twist -
-
-
-
- - relay... - - - - + + - +
-
-
+
+
ndt_scan_matcher.launch.xml

@@ -321,18 +289,18 @@ ndt_scan_matcher.launch.xml... - - + + - +
-
-
+
+
gyro_odometer.launch.xml

@@ -346,18 +314,18 @@ gyro_odometer.launch.xml... - - + + - +
-
-
+
+
ekf_localizer.launch.xml

@@ -371,18 +339,18 @@ ekf_localizer.launch.xml... - - - + + + - +
-
-
+
+
pose_initializer.launch.xml

@@ -393,44 +361,44 @@
- pose_initializer.launch.xml... + pose_initializer.launch.xml... - + - +
-
-
+
+
util.launch.py

- package: tier4_localization_launch + package: localization_launch
- util.launch.py... + util.launch.py... - - - + + + - +
-
-
+
+
crop_box_measurement_range

@@ -441,19 +409,19 @@
- crop_box_measurement_range... + crop_box_measurement_range... - + - +
-
-
+
+
voxel_grid_downsample_filter

@@ -464,19 +432,19 @@
- voxel_grid_downsample_filter... + voxel_grid_downsample_filter... - + - +
-
-
+
+
random_downsample_filter

@@ -487,16 +455,16 @@
- random_downsample_filter... + random_downsample_filter... - - + + - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 7c15e1be21be8..5d060f884c01e 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -12,8 +12,7 @@ Yamato Ando ament_cmake_auto - - autoware_cmake + autoware_cmake automatic_pose_initializer ekf_localizer diff --git a/launch/tier4_map_launch/README.md b/launch/tier4_map_launch/README.md index 5e98593a9bec0..904f20f3a744b 100644 --- a/launch/tier4_map_launch/README.md +++ b/launch/tier4_map_launch/README.md @@ -32,4 +32,4 @@ Note that you should provide parameter paths as `PACKAGE_param_path`. The list o ## Notes -For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS 2 (similar to Nodelet in ROS 1 ) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 955a8658e853e..f9459be3b8924 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -172,7 +172,7 @@ def add_launch_arg(name: str, default_value=None, description=None): None, "path to pointcloud_map_loader param file", ), - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), + add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication"), add_launch_arg("use_multithread", "false", "use multithread"), set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index b4e3303e23c58..6f75950838267 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake map_loader map_tf_generator diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 9ca8ea3df95d2..725c4fad19c48 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -19,7 +19,7 @@ - + @@ -230,7 +230,7 @@ - + @@ -239,7 +239,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 60c9609827589..5ad164d43e98c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -26,7 +26,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ddacff7f38f47..6150f48ba3ab3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index ccb385ff80ca1..bafacc01e6324 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -44,6 +44,7 @@ def __init__(self, context): "map_update_distance_threshold" ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] + self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] def create_pipeline(self): if self.use_down_sample_filter: @@ -72,6 +73,7 @@ def create_normal_pipeline(self): "use_dynamic_map_loading": self.use_dynamic_map_loading, "map_update_distance_threshold": self.map_update_distance_threshold, "map_loader_radius": self.map_loader_radius, + "publish_debug_pcd": self.publish_debug_pcd, "input_frame": "map", } ], @@ -127,6 +129,7 @@ def create_down_sample_pipeline(self): "use_dynamic_map_loading": self.use_dynamic_map_loading, "map_update_distance_threshold": self.map_update_distance_threshold, "map_loader_radius": self.map_loader_radius, + "publish_debug_pcd": self.publish_debug_pcd, "input_frame": "map", } ], diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 3800921f1e443..80ee3963553d6 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -8,15 +8,11 @@ - - - - - - + + - + @@ -25,14 +21,12 @@ - - - + - + @@ -41,9 +35,7 @@ - - - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b0b196c1c2351..75872c2ae7fb2 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -7,6 +7,8 @@ + + @@ -33,7 +35,7 @@ - + @@ -79,8 +81,8 @@ - - + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index 8827dfbb98555..a3fd412577e1f 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -48,6 +48,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "label_file", os.path.join(ssd_fine_detector_share_dir, "data", "voc_labels_tl.txt") ) + add_launch_arg("dnn_header_type", "pytorch") add_launch_arg("fine_detector_precision", "FP32") add_launch_arg("score_thresh", "0.7") add_launch_arg("max_batch_size", "8") @@ -197,6 +198,7 @@ def create_parameter_dict(*args): ssd_fine_detector_param = create_parameter_dict( "onnx_file", "label_file", + "dnn_header_type", "score_thresh", "max_batch_size", "approximate_sync", diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index a55e40a1684fb..1140d643a7083 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -6,11 +6,11 @@ The tier4_perception_launch package Yukihiro Saito Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake compare_map_segmentation crosswalk_traffic_light_estimator diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index a64427b70ec44..6cc3b10d68c5a 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -21,9 +21,9 @@ + - - + @@ -44,6 +44,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 713c2e420f5d4..87481744fdaa4 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -35,6 +35,9 @@ def launch_setup(context, *args, **kwargs): with open(vehicle_param_path, "r") as f: vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # common parameter + with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: + common_param = yaml.safe_load(f)["/**"]["ros__parameters"] # nearest search parameter with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -46,12 +49,12 @@ def launch_setup(context, *args, **kwargs): avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f: avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f: + dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f: lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("lane_following_param_path").perform(context), "r") as f: - lane_following_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("pull_over_param_path").perform(context), "r") as f: - pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f: + goal_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("pull_out_param_path").perform(context), "r") as f: pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: @@ -84,13 +87,14 @@ def launch_setup(context, *args, **kwargs): ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"), ], parameters=[ + common_param, nearest_search_param, side_shift_param, avoidance_param, avoidance_by_lc_param, + dynamic_avoidance_param, lane_change_param, - lane_following_param, - pull_over_param, + goal_planner_param, pull_out_param, drivable_area_expansion_param, scene_module_manager_param, @@ -116,8 +120,6 @@ def launch_setup(context, *args, **kwargs): ) # smoother param - with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: - common_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r" ) as f: @@ -319,7 +321,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_experimental_lane_change_function") # component - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") # for points filter of run out module diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 8d51ffe89a07c..35b8da526e98f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -14,9 +14,9 @@ + - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 03bf4479c8139..d491e14bcf3e4 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -64,6 +64,27 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # path sampler + with open(LaunchConfiguration("path_sampler_param_path").perform(context), "r") as f: + path_sampler_param = yaml.safe_load(f)["/**"]["ros__parameters"] + path_sampler_component = ComposableNode( + package="path_sampler", + plugin="path_sampler::PathSampler", + name="path_sampler", + namespace="", + remappings=[ + ("~/input/path", LaunchConfiguration("input_path_topic")), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/output/path", "obstacle_avoidance_planner/trajectory"), + ], + parameters=[ + nearest_search_param, + path_sampler_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + # obstacle velocity limiter with open( LaunchConfiguration("obstacle_velocity_limiter_param_path").perform(context), "r" @@ -209,7 +230,6 @@ def launch_setup(context, *args, **kwargs): package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ - obstacle_avoidance_planner_component, obstacle_velocity_limiter_component, ], ) @@ -232,6 +252,18 @@ def launch_setup(context, *args, **kwargs): condition=LaunchConfigurationEquals("cruise_planner_type", "none"), ) + obstacle_avoidance_planner_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_avoidance_planner_component], + target_container=container, + condition=LaunchConfigurationEquals("path_planner_type", "obstacle_avoidance_planner"), + ) + + path_sampler_loader = LoadComposableNodes( + composable_node_descriptions=[path_sampler_component], + target_container=container, + condition=LaunchConfigurationEquals("path_planner_type", "path_sampler"), + ) + surround_obstacle_checker_loader = LoadComposableNodes( composable_node_descriptions=[surround_obstacle_checker_component], target_container=container, @@ -241,6 +273,8 @@ def launch_setup(context, *args, **kwargs): group = GroupAction( [ container, + obstacle_avoidance_planner_loader, + path_sampler_loader, obstacle_stop_planner_loader, obstacle_cruise_planner_loader, obstacle_cruise_planner_relay_loader, @@ -273,8 +307,11 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "cruise_planner_type" ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" + add_launch_arg( + "path_planner_type", "obstacle_avoidance_planner" + ) # select from "obstacle_avoidance_planner", "path_sampler" - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index d56fecb4f6a0f..7dfcbb0307bf0 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -136,7 +136,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) # component - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 050968fe70329..69deacf4dfb7f 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -7,7 +7,7 @@ Zulfaqar Azmi - + Kosuke Takeuchi Kosuke Takeuchi @@ -52,8 +52,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake behavior_path_planner behavior_velocity_planner diff --git a/launch/tier4_sensing_launch/package.xml b/launch/tier4_sensing_launch/package.xml index c35082aa7c1be..a1f10ee743db5 100644 --- a/launch/tier4_sensing_launch/package.xml +++ b/launch/tier4_sensing_launch/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake vehicle_info_util diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 56a7541337950..b4cd8d0cf91e2 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -2,6 +2,7 @@ + @@ -15,6 +16,7 @@ + @@ -33,6 +35,7 @@ + @@ -44,9 +47,8 @@ - - - + + diff --git a/launch/tier4_simulator_launch/package.xml b/launch/tier4_simulator_launch/package.xml index 8094f95d78b15..f08d0737b8c40 100644 --- a/launch/tier4_simulator_launch/package.xml +++ b/launch/tier4_simulator_launch/package.xml @@ -12,8 +12,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake dummy_perception_publisher fault_injection diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index c301c17137b9a..5e3fc3ff061f8 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -10,8 +10,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake component_state_monitor emergency_handler diff --git a/launch/tier4_vehicle_launch/package.xml b/launch/tier4_vehicle_launch/package.xml index 501316732d67b..779975d3e1ae1 100644 --- a/launch/tier4_vehicle_launch/package.xml +++ b/launch/tier4_vehicle_launch/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake robot_state_publisher xacro diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 129798e0e8b59..4826b6daa17f1 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -12,9 +12,9 @@ Takamasa Horibe ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake eigen fmt diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp index ceb9d520a75b0..f249dffef6034 100644 --- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp +++ b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp @@ -14,8 +14,8 @@ #include "ekf_localizer/ekf_localizer.hpp" -#include -#include +#include +#include #include #include diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 5aa85a8c4afe1..26781513ff2f7 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -3,14 +3,13 @@ gyro_odometer 0.1.0 - The gyro_odometer package as a ROS2 node + The gyro_odometer package as a ROS 2 node Yamato Ando Apache License 2.0 Yamato Ando ament_cmake_auto - - autoware_cmake + autoware_cmake fmt geometry_msgs diff --git a/localization/initial_pose_button_panel/package.xml b/localization/initial_pose_button_panel/package.xml index f71486a2f337c..2db87fe645fb1 100644 --- a/localization/initial_pose_button_panel/package.xml +++ b/localization/initial_pose_button_panel/package.xml @@ -10,8 +10,7 @@ Yamato ANDO ament_cmake_auto - - autoware_cmake + autoware_cmake geometry_msgs libqt5-core diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index f8a2a69a01558..4c563f0b30ab1 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -10,8 +10,7 @@ Taichi Higashide ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_msgs diagnostic_updater diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 14d3c9e8cca84..b30cbaca1e87a 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -223,14 +223,14 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -| :---------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| splitted | true | true | dynamically | -| splitted | true | false | **does NOT work** | -| splitted | false | true/false | at once (standard) | +| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | +| :------------: | :-----------------------: | :------------------------: | :------------------: | +| single file | true | true | at once (standard) | +| single file | true | false | **does NOT work** | +| single file | false | true/false | at once (standard) | +| multiple files | true | true | dynamically | +| multiple files | true | false | **does NOT work** | +| multiple files | false | true/false | at once (standard) | ## Scan matching score based on de-grounded LiDAR scan 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 fb9577ca42934..f7966879a5cb0 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp index a83e8db02f521..038ed4549eb2f 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp @@ -15,7 +15,7 @@ #ifndef NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ #define NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ -#include +#include using Matrix6d = Eigen::Matrix; using RowMatrixXd = Eigen::Matrix; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index 0e1a6e6816413..d936a97a77ba7 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 581879322caf3..17db6b17d4a45 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -11,8 +11,7 @@ Yamato Ando ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_map_msgs diagnostic_msgs diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 2e8052b41d413..8ea0d38ff1e1b 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -260,7 +260,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); auto sensor_points_mapTF_ptr = std::make_shared>(); - pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); + tier4_autoware_utils::transformPointCloud( + *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); } 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 c72e162e11f67..88e43a8f01980 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -20,6 +20,7 @@ #include "ndt_scan_matcher/util_func.hpp" #include +#include #include @@ -328,6 +329,11 @@ void NDTScanMatcher::callback_regularization_pose( void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr) { + if (sensor_points_sensorTF_msg_ptr->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "Empty sensor points!"); + return; + } + // mutex ndt_ptr_ std::lock_guard lock(ndt_ptr_mtx_); @@ -437,7 +443,7 @@ void NDTScanMatcher::callback_sensor_points( pcl::shared_ptr> sensor_points_mapTF_ptr( new pcl::PointCloud); - pcl::transformPointCloud( + tier4_autoware_utils::transformPointCloud( *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_mapTF_ptr); @@ -495,7 +501,7 @@ void NDTScanMatcher::transform_sensor_measurement( tier4_autoware_utils::transform2pose(*TF_target_to_source_ptr); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); - pcl::transformPointCloud( + tier4_autoware_utils::transformPointCloud( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } diff --git a/localization/ndt_scan_matcher/src/pose_initialization_module.cpp b/localization/ndt_scan_matcher/src/pose_initialization_module.cpp index 899cbb4454f17..2814085fee07c 100644 --- a/localization/ndt_scan_matcher/src/pose_initialization_module.cpp +++ b/localization/ndt_scan_matcher/src/pose_initialization_module.cpp @@ -111,7 +111,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializationModule::align_us ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); auto sensor_points_mapTF_ptr = std::make_shared>(); - pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); + tier4_autoware_utils::transformPointCloud( + *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); } diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 9425983d5b971..ed044bd285909 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -9,8 +9,7 @@ Yamato Ando ament_cmake - - autoware_cmake + autoware_cmake geometry_msgs rclcpp diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 81cfe3c4a0665..961f3a8e3293c 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -15,7 +15,7 @@ This node depends on the map height fitter library. | Name | Type | Description | | --------------------- | ---- | ---------------------------------------------------------------------------------------- | -| `ekf_enabled` | bool | If true, EKF localizar is activated. | +| `ekf_enabled` | bool | If true, EKF localizer is activated. | | `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | | `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | | `stop_check_duration` | bool | The duration used for the stop check above. | diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 86b20833ec4df..e7c28eeea88f9 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -1,13 +1,14 @@ + - + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index de9b54dddecd8..cb519c49e6335 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -11,8 +11,7 @@ Takagi, Isamu ament_cmake - - autoware_cmake + autoware_cmake component_interface_specs component_interface_utils diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index c547a90381d90..b0beeb4758951 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -10,8 +10,7 @@ Koji Minoda ament_cmake_auto - - autoware_cmake + autoware_cmake geometry_msgs nav_msgs diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 8b25f59096759..dc54311b3a1db 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -10,8 +10,7 @@ Koji Minoda ament_cmake_auto - - autoware_cmake + autoware_cmake geometry_msgs nav_msgs diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index dfb6a88944fcf..ab4f5ce205385 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -10,8 +10,7 @@ Takagi, Isamu ament_cmake - - autoware_cmake + autoware_cmake autoware_map_msgs geometry_msgs diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 88f9ade2f40db..7a88ae6812d6d 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -103,11 +103,11 @@ If you have multiple rosbags, an example directory structure would be as follows ```bash sample-map-rosbag ├── lanelet2_map.osm -├── pointcloud_map -├── pcd_00.pcd -├── pcd_01.pcd -├── pcd_02.pcd -├── ... +├── pointcloud_map.pcd +│ ├── A.pcd +│ ├── B.pcd +│ ├── C.pcd +│ └── ... └── pointcloud_map_metadata.yaml ``` @@ -134,7 +134,11 @@ The node projects lan/lon coordinates into MGRS coordinates. ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are 3 types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package. + +- MGRS +- UTM +- local ### How to Run @@ -147,3 +151,13 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Published Topics - ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz + +### Parameters + +| Name | Type | Description | Default value | +| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | +| lanelet2_map_projector_type | std::string | The type of the map projector using, can be MGRS, UTM, local | MGRS | +| latitude | double | Latitude of map_origin, only using in UTM map projector | 0.0 | +| longitude | double | Longitude of map_origin, only using in UTM map projector | 0.0 | +| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | +| lanelet2_map_path | std::string | The lanelet2 map path | None | diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 4345852a0900f..971af0147b0fe 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - lanelet2_map_projector_type: UTM # Options: MGRS, UTM + lanelet2_map_projector_type: MGRS # Options: MGRS, UTM, local latitude: 40.816617984672746 # Latitude of map_origin, using in UTM longitude: 29.360491808334285 # Longitude of map_origin, using in UTM diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 4fa5cc4156794..22cccc16f537f 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_map_msgs diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index da35f74eaa59c..be8ffc94bc296 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -93,6 +93,21 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (errors.empty()) { return map; } + } else if (lanelet2_map_projector_type == "local") { + // Use MGRSProjector as parser + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + + // overwrite local_x, local_y + for (lanelet::Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } + } + return map; } else { RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); return nullptr; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 6e94294c0900f..fad15948c368f 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -121,12 +121,15 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = lanelet::utils::query::getAllPolygonsByType( viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); + lanelet::ConstPolygons3d hatched_road_markings_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, - cl_no_obstacle_segmentation_area_for_run_out; + cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, + cl_hatched_road_markings_line; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -145,6 +148,8 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); visualization_msgs::msg::MarkerArray map_marker_array; @@ -219,6 +224,11 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( + hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); + pub_marker_->publish(map_marker_array); } diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 201d7c6c99c03..6dc68911699aa 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake lanelet2_extension libpcl-all-dev diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 216dffe817449..15f061369e649 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake lanelet2_extension pcl_ros diff --git a/mkdocs.yaml b/mkdocs.yaml index d448ab04c9a41..bf7d2569d83b5 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -1,9 +1,9 @@ site_name: Autoware Universe Documentation site_url: https://autowarefoundation.github.io/autoware.universe repo_url: https://github.com/autowarefoundation/autoware.universe -edit_uri: edit/main/ +edit_uri: https://github.com/autowarefoundation/autoware.universe/edit/main/ docs_dir: . -copyright: Copyright © 2022 The Autoware Foundation +copyright: Copyright © 2023 The Autoware Foundation theme: name: material @@ -54,10 +54,12 @@ plugins: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - macros + - mkdocs-video - same-dir - search markdown_extensions: + - abbr - admonition - attr_list - codehilite: @@ -72,7 +74,14 @@ markdown_extensions: server: http://www.plantuml.com/plantuml format: svg - pymdownx.arithmatex + - pymdownx.details + - pymdownx.emoji: + emoji_index: !!python/name:materialx.emoji.twemoji + emoji_generator: !!python/name:materialx.emoji.to_svg - pymdownx.highlight + - pymdownx.snippets: + auto_append: + - includes/abbreviations.md - pymdownx.superfences: custom_fences: - name: mermaid @@ -80,3 +89,4 @@ markdown_extensions: format: !!python/name:pymdownx.superfences.fence_code_format - toc: permalink: "#" + toc_depth: 3 diff --git a/perception/bytetrack/CMakeLists.txt b/perception/bytetrack/CMakeLists.txt index 3e3d520eef14a..e481464455a55 100644 --- a/perception/bytetrack/CMakeLists.txt +++ b/perception/bytetrack/CMakeLists.txt @@ -29,6 +29,7 @@ target_include_directories(bytetrack_lib $ $ ) +target_link_libraries(bytetrack_lib Eigen3::Eigen) # # ROS node diff --git a/perception/bytetrack/README.md b/perception/bytetrack/README.md index c71b62cada29f..e58f4bb5892aa 100644 --- a/perception/bytetrack/README.md +++ b/perception/bytetrack/README.md @@ -12,6 +12,8 @@ the number of false negatives is expected to decrease by using it. ### Cite + + - Yifu Zhang, Peize Sun, Yi Jiang, Dongdong Yu, Fucheng Weng, Zehuan Yuan, Ping Luo, Wenyu Liu, and Xinggang Wang, "ByteTrack: Multi-Object Tracking by Associating Every Detection Box", in the proc. of the ECCV 2022, [[ref](https://arxiv.org/abs/2110.06864)] diff --git a/perception/bytetrack/include/bytetrack/bytetrack_node.hpp b/perception/bytetrack/include/bytetrack/bytetrack_node.hpp index 6b801ddf93268..bd190cb8005d1 100644 --- a/perception/bytetrack/include/bytetrack/bytetrack_node.hpp +++ b/perception/bytetrack/include/bytetrack/bytetrack_node.hpp @@ -21,7 +21,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/perception/bytetrack/lib/include/data_type.h b/perception/bytetrack/lib/include/data_type.h index d76baa8945637..896e021538cae 100644 --- a/perception/bytetrack/lib/include/data_type.h +++ b/perception/bytetrack/lib/include/data_type.h @@ -38,8 +38,8 @@ #pragma once -#include -#include +#include +#include #include #include diff --git a/perception/bytetrack/lib/src/kalman_filter.cpp b/perception/bytetrack/lib/src/kalman_filter.cpp index bc1a88ff60b0e..e515450858d80 100644 --- a/perception/bytetrack/lib/src/kalman_filter.cpp +++ b/perception/bytetrack/lib/src/kalman_filter.cpp @@ -38,7 +38,7 @@ #include "kalman_filter.h" -#include +#include namespace byte_kalman { diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 3968d6cb97f7f..6b3891438c179 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -8,13 +8,12 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake cudnn_cmake_module cudnn_cmake_module tensorrt_cmake_module - autoware_cmake - autoware_auto_perception_msgs cuda_utils cv_bridge diff --git a/perception/bytetrack/src/bytetrack_visualizer_node.cpp b/perception/bytetrack/src/bytetrack_visualizer_node.cpp index 1ab65272333d3..ce350b8c288da 100644 --- a/perception/bytetrack/src/bytetrack_visualizer_node.cpp +++ b/perception/bytetrack/src/bytetrack_visualizer_node.cpp @@ -17,7 +17,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 5b0ae044b1b12..2d65f75a098a3 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -46,6 +46,8 @@ ament_target_dependencies(compare_map_segmentation sensor_msgs tier4_autoware_utils autoware_map_msgs + component_interface_specs + component_interface_utils ) if(OPENMP_FOUND) diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md index a304fcb67e555..67aceafef3a16 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/compare_map_segmentation/README.md @@ -16,11 +16,11 @@ Compare the z of the input points with the value of elevation_map. The height di ### Distance Based Compare Map Filter -WIP +This filter compares the input pointcloud with the map pointcloud using the `nearestKSearch` function of `kdtree` and removes points that are close to the map point cloud. The map pointcloud can be loaded statically at once at the beginning or dynamically as the vehicle moves. ### Voxel Based Approximate Compare Map Filter -WIP +The filter loads the map point cloud, which can be loaded statically at the beginning or dynamically during vehicle movement, and creates a voxel grid of the map point cloud. The filter uses the getCentroidIndexAt function in combination with the getGridCoordinates function from the VoxelGrid class to find input points that are inside the voxel grid and removes them. ### Voxel Based Compare Map Filter @@ -30,7 +30,7 @@ For each point of input pointcloud, the filter use `getCentroidIndexAt` combine ### Voxel Distance based Compare Map Filter -WIP +This filter is a combination of the distance_based_compare_map_filter and voxel_based_approximate_compare_map_filter. The filter loads the map point cloud, which can be loaded statically at the beginning or dynamically during vehicle movement, and creates a voxel grid and a k-d tree of the map point cloud. The filter uses the getCentroidIndexAt function in combination with the getGridCoordinates function from the VoxelGrid class to find input points that are inside the voxel grid and removes them. For points that do not belong to any voxel grid, they are compared again with the map point cloud using the radiusSearch function of the k-d tree and are removed if they are close enough to the map. ## Inputs / Outputs @@ -61,11 +61,12 @@ WIP #### Input -| Name | Type | Description | -| ------------------------ | ----------------------------------------------- | ------------------------------------------------------ | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | -| `~/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading) | +| Name | Type | Description | +| ------------------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | +| `~/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading) | +| `/localization/initialization_state` | `localization_interface::InitializationState` | Ego-vehicle pose initialization state (in dynamic map loading) | #### Output @@ -75,13 +76,14 @@ WIP #### Parameters -| Name | Type | Description | Default value | -| :------------------------------ | :---- | :------------------------------------------------------------------------------------------- | :------------ | -| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading | true | -| `distance_threshold` | float | VoxelGrid's leaf_size also the threshold to check distance between input point and map point | 0.5 | -| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary [m] | 10.0 | -| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | -| `timer_interval_ms` | int | time interval of timer to check if update the map is necessary (in dynamic map loading) [ms] | 100 | +| Name | Type | Description | Default value | +| :------------------------------ | :---- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud | true | +| `distance_threshold` | float | Threshold distance to compare input points with map points [m] | 0.5 | +| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m] | 10.0 | +| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | +| `timer_interval_ms` | int | Timer interval to check if the map update is necessary (in dynamic map loading) [ms] | 100 | +| `publish_debug_pcd` | bool | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false | ## Assumptions / Known limits diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 613cfd2b177e1..8090ea2e4221a 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -16,33 +16,99 @@ #define COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "voxel_grid_map_loader.hpp" +#include // for pcl::isFinite #include #include +#include +#include #include namespace compare_map_segmentation { + +typedef typename pcl::Filter::PointCloud PointCloud; +typedef typename PointCloud::Ptr PointCloudPtr; +typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader +{ +private: + PointCloudConstPtr map_ptr_; + pcl::search::Search::Ptr tree_; + +public: + DistanceBasedStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + { + RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n"); + } + + void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override; + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; +}; + +class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader +{ +public: + DistanceBasedDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + { + RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; + + inline void addMapCellAndFilter( + const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override + { + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + + pcl::PointCloud map_cell_pc_tmp; + pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); + + auto map_cell_voxel_input_tmp_ptr = + std::make_shared>(map_cell_pc_tmp); + + MapGridVoxelInfo current_voxel_grid_list_item; + current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x; + current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y; + current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x; + current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y; + + // add kdtree + pcl::search::Search::Ptr tree_tmp; + if (!tree_tmp) { + if (map_cell_voxel_input_tmp_ptr->isOrganized()) { + tree_tmp.reset(new pcl::search::OrganizedNeighbor()); + } else { + tree_tmp.reset(new pcl::search::KdTree(false)); + } + } + tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr); + current_voxel_grid_list_item.map_cell_kdtree = tree_tmp; + + // add + (*mutex_ptr_).lock(); + current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); + (*mutex_ptr_).unlock(); + } +}; + class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter { protected: virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - void input_target_callback(const PointCloud2ConstPtr map); - private: - rclcpp::Subscription::SharedPtr sub_map_; - PointCloudConstPtr map_ptr_; double distance_threshold_; - pcl::search::Search::Ptr tree_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + std::unique_ptr distance_based_map_loader_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/multi_voxel_grid_map_update.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/multi_voxel_grid_map_update.hpp deleted file mode 100644 index 4518e9436c3d1..0000000000000 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/multi_voxel_grid_map_update.hpp +++ /dev/null @@ -1,317 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef COMPARE_MAP_SEGMENTATION__MULTI_VOXEL_GRID_MAP_UPDATE_HPP_ -#define COMPARE_MAP_SEGMENTATION__MULTI_VOXEL_GRID_MAP_UPDATE_HPP_ - -#include - -#include - -#include -#include -#include -#include -#include - -#include // for FLT_MAX -#include -#include -#include -#include -#include -#include - -namespace compare_map_segmentation -{ -template -// TODO(badai-nguyen): when map loader I/F is updated, remove this class since -// boundary point calculation become unnecessary -class MultiVoxelGrid : public pcl::VoxelGrid -{ - typedef std::map> MapCellDict; - struct cloud_point_index_idx - { - unsigned int idx; - unsigned int cloud_point_index; - - cloud_point_index_idx() = default; - cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_) - : idx(idx_), cloud_point_index(cloud_point_index_) - { - } - bool operator<(const cloud_point_index_idx & p) const { return (idx < p.idx); } - }; - -protected: - using pcl::VoxelGrid::filter_name_; - using pcl::VoxelGrid::downsample_all_data_; - using pcl::VoxelGrid::input_; - using pcl::VoxelGrid::save_leaf_layout_; - using pcl::VoxelGrid::min_b_; - using pcl::VoxelGrid::max_b_; - using pcl::VoxelGrid::divb_mul_; - using pcl::VoxelGrid::div_b_; - using pcl::VoxelGrid::inverse_leaf_size_; - - using pcl::VoxelGrid::filter_limit_negative_; - using pcl::VoxelGrid::filter_limit_max_; - using pcl::VoxelGrid::filter_limit_min_; - using pcl::VoxelGrid::indices_; - using pcl::VoxelGrid::min_points_per_voxel_; - using pcl::VoxelGrid::filter_field_name_; - - using PointCloud = typename pcl::Filter::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - -public: - using pcl::VoxelGrid::leaf_layout_; - - inline void set_voxel_grid( - std::vector * leaf_layout, const Eigen::Vector4i & min_b, const Eigen::Vector4i & max_b, - const Eigen::Vector4i & div_b, const Eigen::Vector4i & divb_mul, - const Eigen::Array4f & inverse_leaf_size) - { - leaf_layout_ = std::move(*leaf_layout); - min_b_ = min_b; - max_b_ = max_b; - div_b_ = div_b; - divb_mul_ = divb_mul; - inverse_leaf_size_ = inverse_leaf_size; - } - inline Eigen::Vector4f get_min_p() const { return min_p; } - inline Eigen::Vector4f get_max_p() const { return max_p; } - inline Eigen::Vector4i get_min_b() const { return min_b_; } - inline Eigen::Vector4i get_divb_mul() const { return divb_mul_; } - inline Eigen::Vector4i get_max_b() const { return max_b_; } - inline Eigen::Vector4i get_div_b() const { return div_b_; } - inline Eigen::Array4f get_inverse_leaf_size() const { return inverse_leaf_size_; } - inline std::vector getLeafLayout() { return (leaf_layout_); } - - // TODO(badai-nguyen): when map loader I/F is updated, use default Voxel applyFilter since - // boundary point calculation become unnecessary - inline void applyFilter(PointCloud & output) override - { - // Has the input dataset been set already? - if (!input_) { - // pcl::PCL_WARN ("[pcl::applyFilter] No input dataset given!\n"); - output.width = output.height = 0; - output.clear(); - return; - } - - // Copy the header (and thus the frame_id) + allocate enough space for points - output.height = 1; // downsampling breaks the organized structure - output.is_dense = true; // we filter out invalid points - - // Get the minimum and maximum dimensions - if (!filter_field_name_.empty()) // If we don't want to process the entire cloud... - pcl::getMinMax3D( - input_, *indices_, filter_field_name_, static_cast(filter_limit_min_), - static_cast(filter_limit_max_), min_p, max_p, filter_limit_negative_); - else - pcl::getMinMax3D(*input_, *indices_, min_p, max_p); - - // Check that the leaf size is not too small, given the size of the data - std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; - std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; - std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; - - if ((dx * dy * dz) > static_cast(std::numeric_limits::max())) { - // pcl::PCL_WARN("[pcl::applyFilter] Leaf size is too small for the input dataset. Integer - // indices would overflow.\n"); - output = *input_; - return; - } - - // Compute the minimum and maximum bounding box values - min_b_[0] = static_cast(std::floor(min_p[0] * inverse_leaf_size_[0])); - max_b_[0] = static_cast(std::floor(max_p[0] * inverse_leaf_size_[0])); - min_b_[1] = static_cast(std::floor(min_p[1] * inverse_leaf_size_[1])); - max_b_[1] = static_cast(std::floor(max_p[1] * inverse_leaf_size_[1])); - min_b_[2] = static_cast(std::floor(min_p[2] * inverse_leaf_size_[2])); - max_b_[2] = static_cast(std::floor(max_p[2] * inverse_leaf_size_[2])); - - // Compute the number of divisions needed along all axis - div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones(); - div_b_[3] = 0; - - // Set up the division multiplier - divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0); - - // Storage for mapping leaf and pointcloud indexes - std::vector index_vector; - index_vector.reserve(indices_->size()); - - // If we don't want to process the entire cloud, but rather filter points far away from the - // viewpoint first... - if (!filter_field_name_.empty()) { - // Get the distance field index - std::vector fields; - int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); - if (distance_idx == -1) - - // First pass: go over all points and insert them into the index_vector vector - // with calculated idx. Points with the same idx value will contribute to the - // same point of resulting CloudPoint - for (const auto & index : (*indices_)) { - if (!input_->is_dense) - // Check if the point is invalid - if (!pcl::isXYZFinite((*input_)[index])) continue; - - // Get the distance value - const std::uint8_t * pt_data = reinterpret_cast(&(*input_)[index]); - float distance_value = 0; - memcpy(&distance_value, pt_data + fields[distance_idx].offset, sizeof(float)); - - if (filter_limit_negative_) { - // Use a threshold for cutting out points which inside the interval - if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) - continue; - } else { - // Use a threshold for cutting out points which are too close/far away - if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) - continue; - } - - int ijk0 = static_cast( - std::floor((*input_)[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); - int ijk1 = static_cast( - std::floor((*input_)[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); - int ijk2 = static_cast( - std::floor((*input_)[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); - - // Compute the centroid leaf index - int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; - index_vector.emplace_back(static_cast(idx), index); - } - } else { - // No distance filtering, process all data - // First pass: go over all points and insert them into the index_vector vector - // with calculated idx. Points with the same idx value will contribute to the - // same point of resulting CloudPoint - for (const auto & index : (*indices_)) { - if (!input_->is_dense) - // Check if the point is invalid - if (!pcl::isXYZFinite((*input_)[index])) continue; - - int ijk0 = static_cast( - std::floor((*input_)[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); - int ijk1 = static_cast( - std::floor((*input_)[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); - int ijk2 = static_cast( - std::floor((*input_)[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); - - // Compute the centroid leaf index - int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; - index_vector.emplace_back(static_cast(idx), index); - } - } - - // Second pass: sort the index_vector vector using value representing target cell as index - // in effect all points belonging to the same output cell will be next to each other - auto rightshift_func = [](const cloud_point_index_idx & x, const unsigned offset) { - return x.idx >> offset; - }; - boost::sort::spreadsort::integer_sort( - index_vector.begin(), index_vector.end(), rightshift_func); - - // Third pass: count output cells - // we need to skip all the same, adjacent idx values - unsigned int total = 0; - unsigned int index = 0; - // first_and_last_indices_vector[i] represents the index in index_vector of the first point in - // index_vector belonging to the voxel which corresponds to the i-th output point, - // and of the first point not belonging to. - std::vector> first_and_last_indices_vector; - // Worst case size - first_and_last_indices_vector.reserve(index_vector.size()); - while (index < index_vector.size()) { - unsigned int i = index + 1; - while (i < index_vector.size() && index_vector[i].idx == index_vector[index].idx) ++i; - if (i - index >= min_points_per_voxel_) { - ++total; - first_and_last_indices_vector.emplace_back(index, i); - } - index = i; - } - - // Fourth pass: compute centroids, insert them into their final position - output.resize(total); - if (save_leaf_layout_) { - try { - // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it - // needs to be re-initialized to -1 - std::uint32_t new_layout_size = div_b_[0] * div_b_[1] * div_b_[2]; - // This is the number of elements that need to be re-initialized to -1 - std::uint32_t reinit_size = std::min( - static_cast(new_layout_size), - static_cast(leaf_layout_.size())); - for (std::uint32_t i = 0; i < reinit_size; i++) { - leaf_layout_[i] = -1; - } - leaf_layout_.resize(new_layout_size, -1); - } catch (std::bad_alloc &) { - throw pcl::PCLException( - "VoxelGrid bin size is too low; impossible to allocate memory for layout", - "voxel_grid.hpp", "applyFilter"); - } catch (std::length_error &) { - throw pcl::PCLException( - "VoxelGrid bin size is too low; impossible to allocate memory for layout", - "voxel_grid.hpp", "applyFilter"); - } - } - - index = 0; - for (const auto & cp : first_and_last_indices_vector) { - // calculate centroid - sum values from all input points, that have the same idx value in - // index_vector array - unsigned int first_index = cp.first; - unsigned int last_index = cp.second; - - // index is centroid final position in resulting PointCloud - if (save_leaf_layout_) leaf_layout_[index_vector[first_index].idx] = index; - - // Limit downsampling to coords - if (!downsample_all_data_) { - Eigen::Vector4f centroid(Eigen::Vector4f::Zero()); - - for (unsigned int li = first_index; li < last_index; ++li) - centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap(); - - centroid /= static_cast(last_index - first_index); - output[index].getVector4fMap() = centroid; - } else { - pcl::CentroidPoint centroid; - - // fill in the accumulator with leaf points - for (unsigned int li = first_index; li < last_index; ++li) - centroid.add((*input_)[index_vector[li].cloud_point_index]); - - centroid.get(output[index]); - } - - ++index; - } - output.width = output.size(); - } - -private: - Eigen::Vector4f min_p, max_p; -}; - -} // namespace compare_map_segmentation - -#endif // COMPARE_MAP_SEGMENTATION__MULTI_VOXEL_GRID_MAP_UPDATE_HPP_ diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp index 53edda0852cf6..f05f8100873cd 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp @@ -16,35 +16,52 @@ #define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT #include "pointcloud_preprocessor/filter.hpp" +#include "voxel_grid_map_loader.hpp" #include #include +#include +#include #include namespace compare_map_segmentation { + +class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader +{ +public: + explicit VoxelBasedApproximateStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + { + RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; +}; + +class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader +{ +public: + VoxelBasedApproximateDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + { + RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; +}; + class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preprocessor::Filter { protected: virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - void input_target_callback(const PointCloud2ConstPtr map); - private: - // pcl::SegmentDifferences impl_; - rclcpp::Subscription::SharedPtr sub_map_; - PointCloudPtr voxel_map_ptr_; double distance_threshold_; - pcl::VoxelGrid voxel_grid_; - bool set_map_in_voxel_grid_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + std::unique_ptr voxel_based_approximate_map_loader_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index e16f1230a9608..d60e4af75e89c 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -16,14 +16,110 @@ #define COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT #include "pointcloud_preprocessor/filter.hpp" +#include "voxel_grid_map_loader.hpp" #include #include +#include +#include +#include +#include #include namespace compare_map_segmentation { + +typedef typename pcl::Filter::PointCloud PointCloud; +typedef typename PointCloud::Ptr PointCloudPtr; +typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader +{ +private: + PointCloudConstPtr map_ptr_; + pcl::search::Search::Ptr tree_; + +public: + explicit VoxelDistanceBasedStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + { + RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; + void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override; +}; + +class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader +{ +protected: +private: + PointCloudConstPtr map_ptr_; + /* data */ +public: + explicit VoxelDistanceBasedDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + { + RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; + + inline void addMapCellAndFilter( + const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override + { + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + + pcl::PointCloud map_cell_pc_tmp; + pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); + + VoxelGridPointXYZ map_cell_voxel_grid_tmp; + PointCloudPtr map_cell_downsampled_pc_ptr_tmp; + + auto map_cell_voxel_input_tmp_ptr = + std::make_shared>(map_cell_pc_tmp); + map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + map_cell_downsampled_pc_ptr_tmp.reset(new pcl::PointCloud); + map_cell_voxel_grid_tmp.setInputCloud(map_cell_voxel_input_tmp_ptr); + map_cell_voxel_grid_tmp.setSaveLeafLayout(true); + map_cell_voxel_grid_tmp.filter(*map_cell_downsampled_pc_ptr_tmp); + + MapGridVoxelInfo current_voxel_grid_list_item; + current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x; + current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y; + current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x; + current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y; + + current_voxel_grid_list_item.map_cell_voxel_grid.set_voxel_grid( + &(map_cell_voxel_grid_tmp.leaf_layout_), map_cell_voxel_grid_tmp.get_min_b(), + map_cell_voxel_grid_tmp.get_max_b(), map_cell_voxel_grid_tmp.get_div_b(), + map_cell_voxel_grid_tmp.get_divb_mul(), map_cell_voxel_grid_tmp.get_inverse_leaf_size()); + + current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud); + current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp); + + // add kdtree + pcl::search::Search::Ptr tree_tmp; + if (!tree_tmp) { + if (map_cell_voxel_input_tmp_ptr->isOrganized()) { + tree_tmp.reset(new pcl::search::OrganizedNeighbor()); + } else { + tree_tmp.reset(new pcl::search::KdTree(false)); + } + } + tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr); + current_voxel_grid_list_item.map_cell_kdtree = tree_tmp; + + // add + (*mutex_ptr_).lock(); + current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); + (*mutex_ptr_).unlock(); + } +}; + class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter { protected: @@ -33,20 +129,8 @@ class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocess void input_target_callback(const PointCloud2ConstPtr map); private: - // pcl::SegmentDifferences impl_; - rclcpp::Subscription::SharedPtr sub_map_; - PointCloudPtr voxel_map_ptr_; - PointCloudConstPtr map_ptr_; + std::unique_ptr voxel_distance_based_map_loader_; double distance_threshold_; - pcl::search::Search::Ptr tree_; - pcl::VoxelGrid voxel_grid_; - bool set_map_in_voxel_grid_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index f01e01edd1a27..e3f57c8f9b9a0 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -15,22 +15,24 @@ #ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ #define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ -#include "compare_map_segmentation/multi_voxel_grid_map_update.hpp" - +#include +#include #include #include #include #include +#include +#include #include #include #include #include +#include #include #include -// create map loader interface for static and dynamic map template double distance3D(const T p1, const U p2) @@ -49,6 +51,45 @@ double distance2D(const T p1, const U p2) return std::sqrt(dx * dx + dy * dy); } +template +class VoxelGridEx : public pcl::VoxelGrid +{ +protected: + using pcl::VoxelGrid::save_leaf_layout_; + using pcl::VoxelGrid::min_b_; + using pcl::VoxelGrid::max_b_; + using pcl::VoxelGrid::divb_mul_; + using pcl::VoxelGrid::div_b_; + using pcl::VoxelGrid::inverse_leaf_size_; + + using PointCloud = typename pcl::Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + +public: + using pcl::VoxelGrid::leaf_layout_; + + inline void set_voxel_grid( + std::vector * leaf_layout, const Eigen::Vector4i & min_b, const Eigen::Vector4i & max_b, + const Eigen::Vector4i & div_b, const Eigen::Vector4i & divb_mul, + const Eigen::Array4f & inverse_leaf_size) + { + leaf_layout_ = std::move(*leaf_layout); + min_b_ = min_b; + max_b_ = max_b; + div_b_ = div_b; + divb_mul_ = divb_mul; + inverse_leaf_size_ = inverse_leaf_size; + } + + inline Eigen::Vector4i get_min_b() const { return min_b_; } + inline Eigen::Vector4i get_divb_mul() const { return divb_mul_; } + inline Eigen::Vector4i get_max_b() const { return max_b_; } + inline Eigen::Vector4i get_div_b() const { return div_b_; } + inline Eigen::Array4f get_inverse_leaf_size() const { return inverse_leaf_size_; } + inline std::vector getLeafLayout() { return (leaf_layout_); } +}; + class VoxelGridMapLoader { protected: @@ -56,20 +97,24 @@ class VoxelGridMapLoader std::mutex * mutex_ptr_; double voxel_leaf_size_; rclcpp::Publisher::SharedPtr downsampled_map_pub_; + bool debug_ = false; public: - typedef compare_map_segmentation::MultiVoxelGrid MultiVoxelGrid; + typedef VoxelGridEx VoxelGridPointXYZ; typedef typename pcl::Filter::PointCloud PointCloud; typedef typename PointCloud::Ptr PointCloudPtr; explicit VoxelGridMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0; + bool is_close_to_neighbor_voxels( + const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, + pcl::search::Search::Ptr tree) const; bool is_close_to_neighbor_voxels( const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, - MultiVoxelGrid & voxel) const; + VoxelGridPointXYZ & voxel) const; bool is_in_voxel( const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, - const double distance_threshold, const PointCloudPtr & map, MultiVoxelGrid & voxel) const; + const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const; void publish_downsampled_map(const pcl::PointCloud & downsampled_pc); bool is_close_points( @@ -78,37 +123,42 @@ class VoxelGridMapLoader std::string * tf_map_input_frame_; }; -//*************************** for Static Map loader Voxel Grid Filter ************* class VoxelGridStaticMapLoader : public VoxelGridMapLoader { -private: +protected: rclcpp::Subscription::SharedPtr sub_map_; - MultiVoxelGrid voxel_grid_; + VoxelGridPointXYZ voxel_grid_; PointCloudPtr voxel_map_ptr_; public: explicit VoxelGridStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); - void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); - bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); }; -// *************** for Dynamic and Differential Map loader Voxel Grid Filter ************* class VoxelGridDynamicMapLoader : public VoxelGridMapLoader { +protected: struct MapGridVoxelInfo { - MultiVoxelGrid map_cell_voxel_grid; + VoxelGridPointXYZ map_cell_voxel_grid; PointCloudPtr map_cell_pc_ptr; float min_b_x, min_b_y, max_b_x, max_b_y; + pcl::search::Search::Ptr map_cell_kdtree; }; typedef typename std::map VoxelGridDict; + using InitializationState = localization_interface::InitializationState; -private: + /** \brief Map to hold loaded map grid id and it's voxel filter */ VoxelGridDict current_voxel_grid_dict_; + rclcpp::Subscription::SharedPtr sub_estimated_pose_; + component_interface_utils::Subscription::SharedPtr + sub_pose_initializer_state_; + InitializationState::Message initialization_state_; std::optional current_position_ = std::nullopt; std::optional last_updated_position_ = std::nullopt; rclcpp::TimerBase::SharedPtr map_update_timer_; @@ -119,16 +169,41 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader rclcpp::CallbackGroup::SharedPtr client_callback_group_; rclcpp::CallbackGroup::SharedPtr timer_callback_group_; + /** Map grid size. It might be defined by using metadata */ + double map_grid_size_x_ = -1.0; + double map_grid_size_y_ = -1.0; + + double origin_x_remainder_ = 0.0; + double origin_y_remainder_ = 0.0; + + /** \brief Array to hold loaded map grid positions for fast map grid searching. + */ + std::vector> current_voxel_grid_array_; + + /** \brief Array size in x axis */ + int map_grids_x_; + /** \brief Array size in y axis */ + int map_grids_y_; + + /** \brief x-coordinate of map grid which should belong to array[0][0] */ + float origin_x_; + /** \brief y-coordinate of map grid which should belong to array[0][0] */ + float origin_y_; + public: explicit VoxelGridDynamicMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group); void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose); + void onPoseInitializerCallback(const InitializationState::Message::ConstSharedPtr msg); void timer_callback(); bool should_update_map() const; void request_update_map(const geometry_msgs::msg::Point & position); - bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + /** \brief Check if point close to map pointcloud in the */ + bool is_close_to_next_map_grid( + const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold); inline pcl::PointCloud getCurrentDownsampledMapPc() const { @@ -156,6 +231,42 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader for (size_t i = 0; i < map_cell_ids_to_remove.size(); ++i) { removeMapCell(map_cell_ids_to_remove.at(i)); } + + updateVoxelGridArray(); + } + + /** Update loaded map grid array for fast searching*/ + virtual inline void updateVoxelGridArray() + { + origin_x_ = std::floor((current_position_.value().x - map_loader_radius_) / map_grid_size_x_) * + map_grid_size_x_ + + origin_x_remainder_; + origin_y_ = std::floor((current_position_.value().y - map_loader_radius_) / map_grid_size_y_) * + map_grid_size_y_ + + origin_y_remainder_; + + map_grids_x_ = static_cast( + std::ceil((current_position_.value().x + map_loader_radius_ - origin_x_) / map_grid_size_x_)); + map_grids_y_ = static_cast( + std::ceil((current_position_.value().y + map_loader_radius_ - origin_y_) / map_grid_size_y_)); + + if (map_grids_x_ * map_grids_y_ == 0) { + return; + } + + (*mutex_ptr_).lock(); + current_voxel_grid_array_.assign( + map_grids_x_ * map_grid_size_y_, std::make_shared()); + for (const auto & kv : current_voxel_grid_dict_) { + int index = static_cast( + std::floor((kv.second.min_b_x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((kv.second.min_b_y - origin_y_) / map_grid_size_y_)); + if (index >= map_grids_x_ * map_grids_y_) { + continue; + } + current_voxel_grid_array_.at(index) = std::make_shared(kv.second); + } + (*mutex_ptr_).unlock(); } inline void removeMapCell(const std::string map_cell_id_to_remove) @@ -165,13 +276,19 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader (*mutex_ptr_).unlock(); } - inline void addMapCellAndFilter( + virtual inline void addMapCellAndFilter( const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) { + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + + origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_); + origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_); + pcl::PointCloud map_cell_pc_tmp; pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); - MultiVoxelGrid map_cell_voxel_grid_tmp; + VoxelGridPointXYZ map_cell_voxel_grid_tmp; PointCloudPtr map_cell_downsampled_pc_ptr_tmp; auto map_cell_voxel_input_tmp_ptr = @@ -183,11 +300,10 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader map_cell_voxel_grid_tmp.filter(*map_cell_downsampled_pc_ptr_tmp); MapGridVoxelInfo current_voxel_grid_list_item; - // TODO(badai-nguyen): use map cell info from map cell, when map loader I/F is updated - current_voxel_grid_list_item.min_b_x = map_cell_voxel_grid_tmp.get_min_p()[0]; - current_voxel_grid_list_item.min_b_y = map_cell_voxel_grid_tmp.get_min_p()[1]; - current_voxel_grid_list_item.max_b_x = map_cell_voxel_grid_tmp.get_max_p()[0]; - current_voxel_grid_list_item.max_b_y = map_cell_voxel_grid_tmp.get_max_p()[1]; + current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x; + current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y; + current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x; + current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y; current_voxel_grid_list_item.map_cell_voxel_grid.set_voxel_grid( &(map_cell_voxel_grid_tmp.leaf_layout_), map_cell_voxel_grid_tmp.get_min_b(), @@ -195,10 +311,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader map_cell_voxel_grid_tmp.get_divb_mul(), map_cell_voxel_grid_tmp.get_inverse_leaf_size()); current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud); - // for (size_t i = 0; i < map_cell_downsampled_pc_ptr_tmp->points.size(); ++i) { - // current_voxel_grid_list_item.map_cell_pc_ptr->points.push_back( - // map_cell_downsampled_pc_ptr_tmp->points.at(i)); - // } current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp); // add (*mutex_ptr_).lock(); diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index 6db3f0eb3f6a8..fc6806ddaa843 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -3,9 +3,10 @@ compare_map_segmentation 0.1.0 - The ROS2 compare_map_segmentation package + The ROS 2 compare_map_segmentation package amc-nu Yukihiro Saito + badai nguyen Apache License 2.0 @@ -14,10 +15,11 @@ William Woodall ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_map_msgs + component_interface_specs + component_interface_utils grid_map_pcl grid_map_ros pcl_conversions diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index b96b8faa98c99..519afa4145bcf 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -22,52 +22,17 @@ namespace compare_map_segmentation { -using pointcloud_preprocessor::get_param; -DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( - const rclcpp::NodeOptions & options) -: Filter("DistanceBasedCompareMapFilter", options) -{ - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&DistanceBasedCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&DistanceBasedCompareMapFilterComponent::paramCallback, this, _1)); -} - -void DistanceBasedCompareMapFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) -{ - std::scoped_lock lock(mutex_); - if (map_ptr_ == NULL || tree_ == NULL) { - output = *input; - return; - } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - pcl::getPointCloudDifference( - *pcl_input, distance_threshold_ * distance_threshold_, tree_, *pcl_output); - - pcl::toROSMsg(*pcl_output, output); - output.header = input->header; -} - -void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCloud2ConstPtr map) +void DistanceBasedStaticMapLoader::onMapCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::scoped_lock lock(mutex_); + (*mutex_ptr_).lock(); map_ptr_ = map_pcl_ptr; - tf_input_frame_ = map_ptr_->header.frame_id; + *tf_map_input_frame_ = map_ptr_->header.frame_id; if (!tree_) { if (map_ptr_->isOrganized()) { tree_.reset(new pcl::search::OrganizedNeighbor()); @@ -76,23 +41,128 @@ void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCl } } tree_->setInputCloud(map_ptr_); + + (*mutex_ptr_).unlock(); +} + +bool DistanceBasedStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (map_ptr_ == NULL) { + return false; + } + if (tree_ == NULL) { + return false; + } + + std::vector nn_indices(1); + std::vector nn_distances(1); + if (!isFinite(point)) { + return false; + } + if (!tree_->nearestKSearch(point, 1, nn_indices, nn_distances)) { + return false; + } + if (nn_distances[0] > distance_threshold) { + return false; + } + return true; } -rcl_interfaces::msg::SetParametersResult DistanceBasedCompareMapFilterComponent::paramCallback( - const std::vector & p) +bool DistanceBasedDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + if (!isFinite(point)) { + return false; + } + + const int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if (current_voxel_grid_array_.at(map_grid_index) != NULL) { + if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == NULL) { + return false; + } + std::vector nn_indices(1); + std::vector nn_distances(1); + if (!current_voxel_grid_array_.at(map_grid_index) + ->map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) { + return false; + } + if (nn_distances[0] <= distance_threshold) { + return true; + } + } + return false; +} + +DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("DistanceBasedCompareMapFilter", options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } +} + +void DistanceBasedCompareMapFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) { std::scoped_lock lock(mutex_); + stop_watch_ptr_->toc("processing_time", true); - if (get_param(p, "distance_threshold", distance_threshold_)) { - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + + for (size_t i = 0; i < pcl_input->points.size(); ++i) { + if (distance_based_map_loader_->is_close_to_map(pcl_input->points.at(i), distance_threshold_)) { + continue; + } + pcl_output->points.push_back(pcl_input->points.at(i)); } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; - return result; + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } + } // namespace compare_map_segmentation #include diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 3ffed162b65e9..830503467a02a 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -22,7 +22,49 @@ namespace compare_map_segmentation { -using pointcloud_preprocessor::get_param; + +bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) +{ + if (voxel_map_ptr_ == NULL) { + return false; + } + const int index = + voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); + if (index == -1) { + return false; + } else { + return true; + } +} + +bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + + const int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if (current_voxel_grid_array_.at(map_grid_index) != NULL) { + const int index = current_voxel_grid_array_.at(map_grid_index) + ->map_cell_voxel_grid.getCentroidIndexAt( + current_voxel_grid_array_.at(map_grid_index) + ->map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z)); + if (index == -1) { + return false; + } else { + return true; + } + } + return false; +} VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapFilterComponent( const rclcpp::NodeOptions & options) @@ -39,17 +81,17 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF stop_watch_ptr_->tic("processing_time"); } - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); - - set_map_in_voxel_grid_ = false; - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&VoxelBasedApproximateCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VoxelBasedApproximateCompareMapFilterComponent::paramCallback, this, _1)); + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + voxel_based_approximate_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + voxel_based_approximate_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } } void VoxelBasedApproximateCompareMapFilterComponent::filter( @@ -57,43 +99,21 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( PointCloud2 & output) { std::scoped_lock lock(mutex_); - if (voxel_map_ptr_ == NULL) { - output = *input; - return; - } + stop_watch_ptr_->toc("processing_time", true); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); pcl_output->points.reserve(pcl_input->points.size()); for (size_t i = 0; i < pcl_input->points.size(); ++i) { - const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates( - pcl_input->points.at(i).x, pcl_input->points.at(i).y, pcl_input->points.at(i).z)); - if (index == -1) { // empty voxel - // map_ptr_->points.at(index) - pcl_output->points.push_back(pcl_input->points.at(i)); + if (voxel_based_approximate_map_loader_->is_close_to_map( + pcl_input->points.at(i), distance_threshold_)) { + continue; } + pcl_output->points.push_back(pcl_input->points.at(i)); } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; -} - -void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( - const PointCloud2ConstPtr map) -{ - stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud map_pcl; - pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::scoped_lock lock(mutex_); - set_map_in_voxel_grid_ = true; - tf_input_frame_ = map_pcl_ptr->header.frame_id; - voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setInputCloud(map_pcl_ptr); - voxel_grid_.setSaveLeafLayout(true); - voxel_grid_.filter(*voxel_map_ptr_); // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); @@ -105,27 +125,6 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( } } -rcl_interfaces::msg::SetParametersResult -VoxelBasedApproximateCompareMapFilterComponent::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mutex_); - - if (get_param(p, "distance_threshold", distance_threshold_)) { - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setSaveLeafLayout(true); - if (set_map_in_voxel_grid_) { - voxel_grid_.filter(*voxel_map_ptr_); - } - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); - } - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - return result; -} } // namespace compare_map_segmentation #include diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 39330a9641143..5a6ebc401c3d3 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -22,71 +22,26 @@ namespace compare_map_segmentation { -using pointcloud_preprocessor::get_param; -VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent( - const rclcpp::NodeOptions & options) -: Filter("VoxelDistanceBasedCompareMapFilter", options) -{ - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&VoxelDistanceBasedCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VoxelDistanceBasedCompareMapFilterComponent::paramCallback, this, _1)); -} - -void VoxelDistanceBasedCompareMapFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) -{ - std::scoped_lock lock(mutex_); - if (voxel_map_ptr_ == NULL || map_ptr_ == NULL || tree_ == NULL) { - output = *input; - return; - } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates( - pcl_input->points.at(i).x, pcl_input->points.at(i).y, pcl_input->points.at(i).z)); - if (index == -1) { // empty voxel - std::vector nn_indices(1); // nn means nearest neighbor - std::vector nn_distances(1); - if ( - tree_->radiusSearch( - pcl_input->points.at(i), distance_threshold_, nn_indices, nn_distances, 1) == 0) { - pcl_output->points.push_back(pcl_input->points.at(i)); - } - } - } - - pcl::toROSMsg(*pcl_output, output); - output.header = input->header; -} - -void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback( - const PointCloud2ConstPtr map) +void VoxelDistanceBasedStaticMapLoader::onMapCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::scoped_lock lock(mutex_); - tf_input_frame_ = map_pcl_ptr->header.frame_id; + (*mutex_ptr_).lock(); + map_ptr_ = map_pcl_ptr; + *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); + voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); voxel_grid_.setInputCloud(map_pcl_ptr); voxel_grid_.setSaveLeafLayout(true); voxel_grid_.filter(*voxel_map_ptr_); // kdtree map_ptr_ = map_pcl_ptr; + if (!tree_) { if (map_ptr_->isOrganized()) { tree_.reset(new pcl::search::OrganizedNeighbor()); @@ -95,27 +50,109 @@ void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback( } } tree_->setInputCloud(map_ptr_); + (*mutex_ptr_).unlock(); } -rcl_interfaces::msg::SetParametersResult VoxelDistanceBasedCompareMapFilterComponent::paramCallback( - const std::vector & p) +bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) { - std::scoped_lock lock(mutex_); + if (voxel_map_ptr_ == NULL) { + return false; + } + if (map_ptr_ == NULL) { + return false; + } + if (tree_ == NULL) { + return false; + } + if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_grid_, tree_)) { + return true; + } + return false; +} + +bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + + const int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if ( + current_voxel_grid_array_.at(map_grid_index) != NULL && + is_close_to_neighbor_voxels( + point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid, + current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) { + return true; + } + return false; +} + +VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("VoxelDistanceBasedCompareMapFilter", options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "voxel_distance_based_compare_map_filter"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } - if (get_param(p, "distance_threshold", distance_threshold_)) { - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setSaveLeafLayout(true); - if (set_map_in_voxel_grid_) { - voxel_grid_.filter(*voxel_map_ptr_); + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + voxel_distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + voxel_distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } +} + +void VoxelDistanceBasedCompareMapFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + std::scoped_lock lock(mutex_); + stop_watch_ptr_->toc("processing_time", true); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + for (size_t i = 0; i < pcl_input->points.size(); ++i) { + if (voxel_distance_based_map_loader_->is_close_to_map( + pcl_input->points.at(i), distance_threshold_)) { + continue; } - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); + pcl_output->points.push_back(pcl_input->points.at(i)); } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; - return result; + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index d5d820c5a0b31..dc9f965e0696a 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -23,6 +23,7 @@ VoxelGridMapLoader::VoxelGridMapLoader( downsampled_map_pub_ = node->create_publisher( "debug/downsampled_map/pointcloud", rclcpp::QoS{1}.transient_local()); + debug_ = node->declare_parameter("publish_debug_pcd"); } bool VoxelGridMapLoader::is_close_points( @@ -44,9 +45,28 @@ void VoxelGridMapLoader::publish_downsampled_map( downsampled_map_pub_->publish(downsampled_map_msg); } +bool VoxelGridMapLoader::is_close_to_neighbor_voxels( + const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, + pcl::search::Search::Ptr tree) const +{ + const int index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(point.x, point.y, point.z)); + if (index != -1) { + return true; + } + if (tree == NULL) { + return false; + } + std::vector nn_indices(1); + std::vector nn_distances(1); + if (tree->radiusSearch(point, distance_threshold, nn_indices, nn_distances, 1) == 0) { + return false; + } + return true; +} + bool VoxelGridMapLoader::is_close_to_neighbor_voxels( const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, - MultiVoxelGrid & voxel) const + VoxelGridPointXYZ & voxel) const { // check map downsampled pc if (map == NULL) { @@ -202,7 +222,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( bool VoxelGridMapLoader::is_in_voxel( const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, - const double distance_threshold, const PointCloudPtr & map, MultiVoxelGrid & voxel) const + const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const { int voxel_index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z)); @@ -218,8 +238,6 @@ bool VoxelGridMapLoader::is_in_voxel( return false; } -//*************************** for Static Map loader Voxel Grid Filter ************* - VoxelGridStaticMapLoader::VoxelGridStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) : VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) @@ -244,7 +262,10 @@ void VoxelGridStaticMapLoader::onMapCallback( voxel_grid_.setSaveLeafLayout(true); voxel_grid_.filter(*voxel_map_ptr_); (*mutex_ptr_).unlock(); - publish_downsampled_map(*voxel_map_ptr_); + + if (debug_) { + publish_downsampled_map(*voxel_map_ptr_); + } } bool VoxelGridStaticMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) @@ -254,7 +275,6 @@ bool VoxelGridStaticMapLoader::is_close_to_map( } return false; } -//*************** for Dynamic and Differential Map loader Voxel Grid Filter ************* VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, @@ -267,6 +287,10 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( auto main_sub_opt = rclcpp::SubscriptionOptions(); main_sub_opt.callback_group = main_callback_group; + const auto localization_node = component_interface_utils::NodeAdaptor(node); + localization_node.init_sub( + sub_pose_initializer_state_, this, &VoxelGridDynamicMapLoader::onPoseInitializerCallback); + sub_estimated_pose_ = node->create_subscription( "pose_with_covariance", rclcpp::QoS{1}, std::bind(&VoxelGridDynamicMapLoader::onEstimatedPoseCallback, this, std::placeholders::_1), @@ -282,17 +306,47 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( RCLCPP_INFO(logger_, "service not available, waiting again ..."); } - timer_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - map_update_timer_ = node->create_wall_timer( - std::chrono::milliseconds(timer_interval_ms), - std::bind(&VoxelGridDynamicMapLoader::timer_callback, this), timer_callback_group_); + const auto period_ns = rclcpp::Rate(timer_interval_ms).period(); + map_update_timer_ = rclcpp::create_timer( + node, node->get_clock(), period_ns, std::bind(&VoxelGridDynamicMapLoader::timer_callback, this), + timer_callback_group_); +} +void VoxelGridDynamicMapLoader::onPoseInitializerCallback( + const InitializationState::Message::ConstSharedPtr msg) +{ + initialization_state_.state = msg->state; + if (msg->state != InitializationState::Message::INITIALIZED) { + current_position_ = std::nullopt; + last_updated_position_ = std::nullopt; + RCLCPP_INFO(logger_, "Initializing pose... Reset the position of Vehicle"); + } } - void VoxelGridDynamicMapLoader::onEstimatedPoseCallback( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) { current_position_ = msg->pose.pose.position; } +bool VoxelGridDynamicMapLoader::is_close_to_next_map_grid( + const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold) +{ + int neighbor_map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if ( + static_cast(neighbor_map_grid_index) >= current_voxel_grid_array_.size() || + neighbor_map_grid_index == current_map_grid_index || + current_voxel_grid_array_.at(neighbor_map_grid_index) != NULL) { + return false; + } + if (is_close_to_neighbor_voxels( + point, distance_threshold, + current_voxel_grid_array_.at(neighbor_map_grid_index)->map_cell_pc_ptr, + current_voxel_grid_array_.at(neighbor_map_grid_index)->map_cell_voxel_grid)) { + return true; + } + return false; +} bool VoxelGridDynamicMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) @@ -300,37 +354,56 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( if (current_voxel_grid_dict_.size() == 0) { return false; } - std::vector neighbor_map_cells_id; - for (auto & kv : current_voxel_grid_dict_) { - if (point.x < kv.second.min_b_x - distance_threshold) { - continue; - } - if (point.y < kv.second.min_b_y - distance_threshold) { - continue; - } - if (point.x > kv.second.max_b_x + distance_threshold) { - continue; - } - if (point.y > kv.second.max_b_y + distance_threshold) { - continue; - } - // the map cell is found - neighbor_map_cells_id.push_back(kv.first); - // check distance - if (kv.second.map_cell_pc_ptr == NULL) { - continue; - } - if (is_close_to_neighbor_voxels( - point, distance_threshold, kv.second.map_cell_pc_ptr, kv.second.map_cell_voxel_grid)) { - return true; - } + // Compare point with map grid that point belong to + + int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if ( + current_voxel_grid_array_.at(map_grid_index) != NULL && + is_close_to_neighbor_voxels( + point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr, + current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) { + return true; + } + + // Compare point with the neighbor map cells if point close to map cell boundary + + if (is_close_to_next_map_grid( + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z), map_grid_index, + distance_threshold)) { + return true; + } + + if (is_close_to_next_map_grid( + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z), map_grid_index, + distance_threshold)) { + return true; + } + + if (is_close_to_next_map_grid( + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z), map_grid_index, + distance_threshold)) { + return true; } + if (is_close_to_next_map_grid( + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z), map_grid_index, + distance_threshold)) { + return true; + } + return false; } void VoxelGridDynamicMapLoader::timer_callback() { - if (current_position_ == std::nullopt) { + if ( + current_position_ == std::nullopt || + initialization_state_.state != InitializationState::Message::INITIALIZED) { return; } if (last_updated_position_ == std::nullopt) { @@ -375,14 +448,16 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi } status = result.wait_for(std::chrono::seconds(1)); } - // if (status == std::future_status::ready) { if ( - result.get()->new_pointcloud_with_ids.size() > 0 || result.get()->ids_to_remove.size() > 0) { - updateDifferentialMapCells( - result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); + result.get()->new_pointcloud_with_ids.size() == 0 && + result.get()->ids_to_remove.size() == 0) { + return; + } + updateDifferentialMapCells(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); + if (debug_) { + publish_downsampled_map(getCurrentDownsampledMapPc()); } } - publish_downsampled_map(getCurrentDownsampledMapPc()); } diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index d4147643eb7bd..1a3b90f617f4e 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_perception_msgs diff --git a/perception/detected_object_feature_remover/package.xml b/perception/detected_object_feature_remover/package.xml index b609ff0257f1e..44571a3c18dd4 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/detected_object_feature_remover/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs rclcpp diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index f13f05c614620..a63ecdec8cc4f 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -3,14 +3,15 @@ detected_object_validation 1.0.0 - The ROS2 detected_object_validation package + The ROS 2 detected_object_validation package Yukihiro Saito + Shunsuke Miura Apache License 2.0 ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake libopencv-dev autoware_auto_mapping_msgs diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index 1f31dfd6d5fc8..44d62e37e958c 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -3,15 +3,15 @@ detection_by_tracker 1.0.0 - The ROS2 detection_by_tracker package + The ROS 2 detection_by_tracker package Yukihiro Saito + Yoshi Ri Apache License 2.0 ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake - eigen euclidean_cluster libpcl-all-dev diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 0e2ea81f9f28e..6e271b8528c47 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -9,8 +9,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_map_msgs diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml index b74608d505b35..f14e869e5296f 100644 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ b/perception/euclidean_cluster/config/compare_map.param.yaml @@ -1,3 +1,17 @@ /**: ros__parameters: distance_threshold: 0.35 + # publish voxelized map pointcloud for debug + publish_debug_pcd: False + + # use dynamic map loading + use_dynamic_map_loading: True + + # time interval to check dynamic map loading + timer_interval_ms: 100 + + # distance threshold for dynamic map update + map_update_distance_threshold: 10.0 + + # radius map for dynamic map loading + map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index cf48bd088d378..bd1495573cdbc 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -56,6 +56,19 @@ def load_composable_node_param(param_path): ("input", "voxel_grid_filtered/pointcloud"), ("map", LaunchConfiguration("input_map")), ("output", "compare_map_filtered/pointcloud"), + ("map_loader_service", "/map/get_differential_pointcloud_map"), + ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ], + parameters=[ + { + "distance_threshold": 0.5, + "timer_interval_ms": 100, + "use_dynamic_map_loading": True, + "map_update_distance_threshold": 10.0, + "map_loader_radius": 150.0, + "publish_debug_pcd": True, + "input_frame": "map", + } ], ) diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index f523245092564..20a9c3cc89a8f 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -45,11 +45,13 @@ def load_composable_node_param(param_path): ("input", LaunchConfiguration("input_pointcloud")), ("map", LaunchConfiguration("input_map")), ("output", "map_filter/pointcloud"), + ("map_loader_service", "/map/get_differential_pointcloud_map"), + ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), ], parameters=[load_composable_node_param("compare_map_param_path")], ) - # separate range of poincloud when map_filter used + # separate range of pointcloud when map_filter used use_map_short_range_crop_box_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index d99c923d91928..56be3c3fc5bc0 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs compare_map_segmentation diff --git a/perception/front_vehicle_velocity_estimator/package.xml b/perception/front_vehicle_velocity_estimator/package.xml index 32cf88555804f..3b1af8cf544ac 100644 --- a/perception/front_vehicle_velocity_estimator/package.xml +++ b/perception/front_vehicle_velocity_estimator/package.xml @@ -10,6 +10,7 @@ Satoshi Tanaka ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs geometry_msgs @@ -22,7 +23,6 @@ sensor_msgs tier4_autoware_utils - autoware_cmake ament_lint_auto autoware_lint_common diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index cdbab9ee5bd66..c44f30aa08998 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -3,7 +3,7 @@ ground_segmentation 0.1.0 - The ROS2 ground_segmentation package + The ROS 2 ground_segmentation package amc-nu Yukihiro Saito Shunsuke Miura @@ -16,8 +16,7 @@ William Woodall ament_cmake_auto - - autoware_cmake + autoware_cmake libopencv-dev pcl_conversions diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index bdc01106899fb..4b2e81d272e30 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -244,7 +244,7 @@ void RayGroundFilterComponent::ClassifyPointCloud( } } -// [ROS2-port]: removed +// [ROS 2-port]: removed // bool RayGroundFilterComponent::child_init() // { // // Enable the dynamic reconfigure service diff --git a/perception/heatmap_visualizer/package.xml b/perception/heatmap_visualizer/package.xml index 7578fe4ba2df5..0a7db1ae42786 100644 --- a/perception/heatmap_visualizer/package.xml +++ b/perception/heatmap_visualizer/package.xml @@ -9,8 +9,7 @@ Kotaro Uetake ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs geometry_msgs diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index dca8795b7ca05..1bbee35ab44f8 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -40,8 +40,8 @@ if the roi msgs can be matched, fuse them and cache the pointcloud. | :-----------------: | :--------: | :-------: | :-------: | :-------: | | subscription status | O | O | O | | -If the roi msg 3 is subscribed before the next pointcloud messge coming or timeout, fuse it if matched, otherwise wait for the next roi msg 3. -If the roi msg 3 is not subscribed before the next pointcloud messge coming or timeout, postprocess the pointcloud messege as it is. +If the roi msg 3 is subscribed before the next pointcloud message coming or timeout, fuse it if matched, otherwise wait for the next roi msg 3. +If the roi msg 3 is not subscribed before the next pointcloud message coming or timeout, postprocess the pointcloud message as it is. The timeout threshold should be set according to the postprocessing time. E.g, if the postprocessing time is around 50ms, the timeout threshold should be set smaller than 50ms, so that the whole processing time could be less than 100ms. diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index e2b141542b921..ef2c4644ae444 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -9,8 +9,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs cv_bridge diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index 93b7c6b52986a..b22c70dfcb9c9 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -14,7 +14,11 @@ #include "image_projection_based_fusion/debugger.hpp" +#if __has_include() +#include +#else #include +#endif namespace { diff --git a/perception/lidar_apollo_instance_segmentation/README.md b/perception/lidar_apollo_instance_segmentation/README.md index 43ecae0880136..2d86b695d2474 100644 --- a/perception/lidar_apollo_instance_segmentation/README.md +++ b/perception/lidar_apollo_instance_segmentation/README.md @@ -140,4 +140,4 @@ Supported lidars are velodyne 16, 64 and 128, but you can also use velodyne 32 a - [Apollo Project](https://github.com/ApolloAuto/apollo) - [lewes6369](https://github.com/lewes6369) - [Autoware Foundation](https://github.com/autowarefoundation/autoware) -- [Kosuke Takeuchi](https://github.com/kosuke55) (TierIV Part timer) +- [Kosuke Takeuchi](https://github.com/kosuke55) (TIER IV) diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp index 5686ce5cec34f..ca2ae95149e9a 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp @@ -23,8 +23,8 @@ #include #include #include +#include -#include #include #include diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index e28b2a941a95a..5da7825f96681 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -83,7 +83,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500)); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); + tier4_autoware_utils::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); transformed_cloud.header.frame_id = target_frame_; pcl_transformed_cloud.header.frame_id = target_frame_; } catch (tf2::TransformException & ex) { @@ -98,7 +98,8 @@ bool LidarApolloInstanceSegmentation::transformCloud( pcl::PointCloud pointcloud_with_z_offset; Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - pcl::transformPointCloud(pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); + tier4_autoware_utils::transformPointCloud( + pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud); diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp index 254e5ddd91b64..35655cd16cb7c 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp @@ -15,7 +15,6 @@ #ifndef LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ #define LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ -#include #include #include #include @@ -28,6 +27,7 @@ #include #include +#include #include #include @@ -37,9 +37,6 @@ namespace perception { namespace lidar_apollo_segmentation_tvm { -using autoware::common::types::bool8_t; -using autoware::common::types::char8_t; -using autoware::common::types::float32_t; /// \brief Internal obstacle classification categories. enum class MetaType { @@ -56,11 +53,11 @@ struct Obstacle { std::vector grids; pcl::PointCloud::Ptr cloud_ptr; - float32_t score; - float32_t height; - float32_t heading; + float score; + float height; + float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probs; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) { @@ -78,7 +75,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D /// \param[in] rows The number of rows in the cluster. /// \param[in] cols The number of columns in the cluster. /// \param[in] range Scaling factor. - explicit Cluster2D(int32_t rows, int32_t cols, float32_t range); + explicit Cluster2D(int32_t rows, int32_t cols, float range); /// \brief Construct a directed graph and search the connected components for candidate object /// clusters. @@ -88,17 +85,17 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D /// \param[in] objectness_thresh Threshold for filtering out non-object cells. /// \param[in] use_all_grids_for_clustering void cluster( - const float32_t * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, - const pcl::PointIndices & valid_indices, float32_t objectness_thresh, - bool8_t use_all_grids_for_clustering); + const float * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, + const pcl::PointIndices & valid_indices, float objectness_thresh, + bool use_all_grids_for_clustering); /// \brief Populate the fields of obstacles_ elements. /// \param[in] inferred_data Prediction information from the neural network inference. - void filter(const float32_t * inferred_data); + void filter(const float * inferred_data); /// \brief Assign a classification type to the obstacles_ elements. /// \param[in] inferred_data Prediction information from the neural network inference. - void classify(const float32_t * inferred_data); + void classify(const float * inferred_data); /// \brief Remove the candidate clusters that don't meet the parameters' requirements. /// \param[in] confidence_thresh The detection confidence score threshold. @@ -107,7 +104,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. /// \return The detected objects. std::shared_ptr getObjects( - float32_t confidence_thresh, float32_t height_thresh, int32_t min_pts_num); + float confidence_thresh, float height_thresh, int32_t min_pts_num); /// \brief Transform an obstacle from the internal representation to the external one. /// \param[in] in_obstacle @@ -119,10 +116,10 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D const int32_t rows_; const int32_t cols_; const int32_t siz_; - const float32_t range_; - const float32_t scale_; - const float32_t inv_res_x_; - const float32_t inv_res_y_; + const float range_; + const float scale_; + const float inv_res_x_; + const float inv_res_y_; std::vector point2grid_; std::vector obstacles_; std::vector id_img_; @@ -135,10 +132,10 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D { Node * center_node; Node * parent; - char8_t node_rank; - char8_t traversed; - bool8_t is_center; - bool8_t is_object; + int8_t node_rank; + int8_t traversed; + bool is_center; + bool is_object; int32_t point_num; int32_t obstacle_id; diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp index ee880e8151be9..5cecda966d198 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp @@ -15,7 +15,6 @@ #ifndef LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ #define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ -#include #include #include #include @@ -33,17 +32,15 @@ namespace perception { namespace lidar_apollo_segmentation_tvm { -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; /// \brief A FeatureMap generator based on channel feature information. class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator { private: - const bool8_t use_intensity_feature_; - const bool8_t use_constant_feature_; - const float32_t min_height_; - const float32_t max_height_; + const bool use_intensity_feature_; + const bool use_constant_feature_; + const float min_height_; + const float max_height_; std::shared_ptr map_ptr_; public: @@ -56,8 +53,8 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator /// \param[in] min_height The minimum height. /// \param[in] max_height The maximum height. explicit FeatureGenerator( - int32_t width, int32_t height, int32_t range, bool8_t use_intensity_feature, - bool8_t use_constant_feature, float32_t min_height, float32_t max_height); + int32_t width, int32_t height, int32_t range, bool use_intensity_feature, + bool use_constant_feature, float min_height, float max_height); /// \brief Generate a FeatureMap based on the configured features of this object. /// \param[in] pc_ptr Pointcloud used to populate the generated FeatureMap. diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp index f8525176a5675..349dfc7f1f0ad 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp @@ -15,8 +15,6 @@ #ifndef LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ #define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ -#include - #include #include @@ -26,7 +24,6 @@ namespace perception { namespace lidar_apollo_segmentation_tvm { -using autoware::common::types::float32_t; /// \brief Abstract interface for FeatureMap. struct FeatureMapInterface @@ -36,17 +33,17 @@ struct FeatureMapInterface const int32_t width; const int32_t height; const int32_t range; - float32_t * max_height_data; // channel 0 - float32_t * mean_height_data; // channel 1 - float32_t * count_data; // channel 2 - float32_t * direction_data; // channel 3 - float32_t * top_intensity_data; // channel 4 - float32_t * mean_intensity_data; // channel 5 - float32_t * distance_data; // channel 6 - float32_t * nonempty_data; // channel 7 - std::vector map_data; - virtual void initializeMap(std::vector & map) = 0; - virtual void resetMap(std::vector & map) = 0; + float * max_height_data; // channel 0 + float * mean_height_data; // channel 1 + float * count_data; // channel 2 + float * direction_data; // channel 3 + float * top_intensity_data; // channel 4 + float * mean_intensity_data; // channel 5 + float * distance_data; // channel 6 + float * nonempty_data; // channel 7 + std::vector map_data; + virtual void initializeMap(std::vector & map) = 0; + virtual void resetMap(std::vector & map) = 0; explicit FeatureMapInterface(int32_t _channels, int32_t _width, int32_t _height, int32_t _range); }; @@ -54,32 +51,32 @@ struct FeatureMapInterface struct FeatureMap : public FeatureMapInterface { explicit FeatureMap(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; }; /// \brief FeatureMap with an intensity feature channel. struct FeatureMapWithIntensity : public FeatureMapInterface { explicit FeatureMapWithIntensity(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; }; /// \brief FeatureMap with a constant feature channel. struct FeatureMapWithConstant : public FeatureMapInterface { explicit FeatureMapWithConstant(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; }; /// \brief FeatureMap with constant and intensity feature channels. struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface { explicit FeatureMapWithConstantAndIntensity(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; }; } // namespace lidar_apollo_segmentation_tvm } // namespace perception diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp index 089381ab51c61..b9734a41d28ae 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp @@ -15,7 +15,6 @@ #ifndef LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ #define LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ -#include #include #include #include @@ -36,6 +35,7 @@ #include #include +#include #include #include #include @@ -46,8 +46,7 @@ namespace perception { namespace lidar_apollo_segmentation_tvm { -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; + using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tvm_utility::pipeline::TVMArrayContainer; using tvm_utility::pipeline::TVMArrayContainerVector; @@ -66,8 +65,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPreProcessor /// \param[in] max_height The maximum height. explicit ApolloLidarSegmentationPreProcessor( const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, - bool8_t use_intensity_feature, bool8_t use_constant_feature, float32_t min_height, - float32_t max_height); + bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height); /// \brief Transfer the input data to a TVM array. /// \param[in] pc_ptr Input pointcloud. @@ -103,8 +101,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor explicit ApolloLidarSegmentationPostProcessor( const tvm_utility::pipeline::InferenceEngineTVMConfig & config, const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, - float32_t objectness_thresh, float32_t score_threshold, float32_t height_thresh, - int32_t min_pts_num); + float objectness_thresh, float score_threshold, float height_thresh, int32_t min_pts_num); /// \brief Copy the inference result. /// \param[in] input The result of the inference engine. @@ -116,9 +113,9 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor const int64_t output_width; const int64_t output_height; const int64_t output_datatype_bytes; - const float32_t objectness_thresh_; - const float32_t score_threshold_; - const float32_t height_thresh_; + const float objectness_thresh_; + const float score_threshold_; + const float height_thresh_; const int32_t min_pts_num_; const pcl::PointCloud::ConstPtr pc_ptr_; const std::shared_ptr cluster2d_; @@ -145,9 +142,9 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation /// object height by height_thresh are filtered out in the /// post-processing step. explicit ApolloLidarSegmentation( - int32_t range, float32_t score_threshold, bool8_t use_intensity_feature, - bool8_t use_constant_feature, float32_t z_offset, float32_t min_height, float32_t max_height, - float32_t objectness_thresh, int32_t min_pts_num, float32_t height_thresh); + int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature, + float z_offset, float min_height, float max_height, float objectness_thresh, + int32_t min_pts_num, float height_thresh); /// \brief Detect obstacles. /// \param[in] input Input pointcloud. @@ -167,14 +164,14 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation private: const int32_t range_; - const float32_t score_threshold_; - const float32_t z_offset_; - const float32_t objectness_thresh_; + const float score_threshold_; + const float z_offset_; + const float objectness_thresh_; const int32_t min_pts_num_; - const float32_t height_thresh_; + const float height_thresh_; const pcl::PointCloud::Ptr pcl_pointcloud_ptr_; // Earliest supported model version. - const std::array model_version_from{2, 0, 0}; + const std::array model_version_from{2, 0, 0}; // Pipeline using PrePT = ApolloLidarSegmentationPreProcessor; @@ -194,7 +191,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation /// \throw tf2::TransformException If the pointcloud transformation fails. void LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL transformCloud( const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, - float32_t z_offset); + float z_offset); rclcpp::Clock::SharedPtr clock_ = std::make_shared(RCL_ROS_TIME); std::unique_ptr tf_buffer_ = std::make_unique(clock_); diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp index 7121e893795de..85502267090e2 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp @@ -15,20 +15,17 @@ #ifndef LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ #define LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ -#include - namespace autoware { namespace perception { namespace lidar_apollo_segmentation_tvm { -using autoware::common::types::float32_t; /// \brief Use a lookup table to compute the natural logarithm of 1+num. /// \param[in] num /// \return ln(1+num) -float32_t calcApproximateLog(float32_t num); +float calcApproximateLog(float num); } // namespace lidar_apollo_segmentation_tvm } // namespace perception } // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp index 1b138f15a7ba5..e8e53d0bb30f2 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp @@ -15,8 +15,6 @@ #ifndef LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ #define LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ -#include - #include #include @@ -26,14 +24,13 @@ namespace perception { namespace lidar_apollo_segmentation_tvm { -using autoware::common::types::float32_t; /// \brief Project a point from a pointcloud to a 2D map. /// \param[in] val Coordinate of the point in the pointcloud. /// \param[in] ori Diameter of area containing the pointcloud. /// \param[in] scale Scaling factor from pointcloud size to grid size. /// \return The grid in which the point is. -inline int32_t F2I(float32_t val, float32_t ori, float32_t scale) +inline int32_t F2I(float val, float ori, float scale) { return static_cast(std::floor((ori - val) * scale)); } @@ -43,9 +40,9 @@ inline int32_t F2I(float32_t val, float32_t ori, float32_t scale) /// \param[in] in_range Range of the pointcloud. /// \param[in] out_size Size of the grid. /// \return The distance to the point in pixel scale. -inline int32_t Pc2Pixel(float32_t in_pc, float32_t in_range, float32_t out_size) +inline int32_t Pc2Pixel(float in_pc, float in_range, float out_size) { - float32_t inv_res = 0.5f * out_size / in_range; + float inv_res = 0.5f * out_size / in_range; return static_cast(std::floor((in_range - in_pc) * inv_res)); } @@ -54,10 +51,10 @@ inline int32_t Pc2Pixel(float32_t in_pc, float32_t in_range, float32_t out_size) /// \param[in] in_size Size of the grid. /// \param[in] out_range Range of the pointcloud. /// \return The distance to the cell in pointcloud scale. -inline float32_t Pixel2Pc(int32_t in_pixel, float32_t in_size, float32_t out_range) +inline float Pixel2Pc(int32_t in_pixel, float in_size, float out_range) { - float32_t res = 2.0f * out_range / in_size; - return out_range - (static_cast(in_pixel) + 0.5f) * res; + float res = 2.0f * out_range / in_size; + return out_range - (static_cast(in_pixel) + 0.5f) * res; } } // namespace lidar_apollo_segmentation_tvm } // namespace perception diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml index 788c3423a4f4f..f4f283abe9450 100755 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -9,17 +9,17 @@ Apache 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake libpcl-all-dev - autoware_auto_common autoware_auto_perception_msgs geometry_msgs pcl_conversions rclcpp sensor_msgs tf2_eigen + tf2_geometry_msgs tf2_ros tier4_perception_msgs tvm_utility diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp index ef06660fdd653..39c0da04a9c39 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -33,8 +32,6 @@ #include using Point = geometry_msgs::msg::Point32; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; namespace autoware { @@ -49,14 +46,14 @@ geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double return tf2::toMsg(q); } -Cluster2D::Cluster2D(const int32_t rows, const int32_t cols, const float32_t range) +Cluster2D::Cluster2D(const int32_t rows, const int32_t cols, const float range) : rows_(rows), cols_(cols), siz_(rows * cols), range_(range), - scale_(0.5f * static_cast(rows) / range), - inv_res_x_(0.5f * static_cast(cols) / range), - inv_res_y_(0.5f * static_cast(rows) / range) + scale_(0.5f * static_cast(rows) / range), + inv_res_x_(0.5f * static_cast(cols) / range), + inv_res_y_(0.5f * static_cast(rows) / range) { point2grid_.clear(); id_img_.assign(siz_, -1); @@ -88,13 +85,13 @@ void Cluster2D::traverse(Node * x) const } void Cluster2D::cluster( - const float32_t * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, - const pcl::PointIndices & valid_indices, float32_t objectness_thresh, + const float * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, + const pcl::PointIndices & valid_indices, float objectness_thresh, bool use_all_grids_for_clustering) { - const float32_t * category_pt_data = inferred_data; - const float32_t * instance_pt_x_data = inferred_data + siz_; - const float32_t * instance_pt_y_data = inferred_data + siz_ * 2; + const float * category_pt_data = inferred_data; + const float * instance_pt_x_data = inferred_data + siz_; + const float * instance_pt_y_data = inferred_data + siz_ * 2; pc_ptr_ = pc_ptr; @@ -185,35 +182,35 @@ void Cluster2D::cluster( classify(inferred_data); } -void Cluster2D::filter(const float32_t * inferred_data) +void Cluster2D::filter(const float * inferred_data) { - const float32_t * confidence_pt_data = inferred_data + siz_ * 3; - const float32_t * heading_pt_x_data = inferred_data + siz_ * 9; - const float32_t * heading_pt_y_data = inferred_data + siz_ * 10; - const float32_t * height_pt_data = inferred_data + siz_ * 11; + const float * confidence_pt_data = inferred_data + siz_ * 3; + const float * heading_pt_x_data = inferred_data + siz_ * 9; + const float * heading_pt_y_data = inferred_data + siz_ * 10; + const float * height_pt_data = inferred_data + siz_ * 11; for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { Obstacle * obs = &obstacles_[obstacle_id]; - float64_t score = 0.0; - float64_t height = 0.0; - float64_t vec_x = 0.0; - float64_t vec_y = 0.0; + double score = 0.0; + double height = 0.0; + double vec_x = 0.0; + double vec_y = 0.0; for (int32_t grid : obs->grids) { - score += static_cast(confidence_pt_data[grid]); - height += static_cast(height_pt_data[grid]); + score += static_cast(confidence_pt_data[grid]); + height += static_cast(height_pt_data[grid]); vec_x += heading_pt_x_data[grid]; vec_y += heading_pt_y_data[grid]; } - obs->score = static_cast(score / static_cast(obs->grids.size())); - obs->height = static_cast(height / static_cast(obs->grids.size())); - obs->heading = static_cast(std::atan2(vec_y, vec_x) * 0.5); + obs->score = static_cast(score / static_cast(obs->grids.size())); + obs->height = static_cast(height / static_cast(obs->grids.size())); + obs->heading = static_cast(std::atan2(vec_y, vec_x) * 0.5); obs->cloud_ptr.reset(new pcl::PointCloud); } } -void Cluster2D::classify(const float32_t * inferred_data) +void Cluster2D::classify(const float * inferred_data) { - const float32_t * classify_pt_data = inferred_data + siz_ * 4; + const float * classify_pt_data = inferred_data + siz_ * 4; int num_classes = static_cast(MetaType::MAX_META_TYPE); for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { Obstacle * obs = &obstacles_[obs_id]; @@ -226,7 +223,7 @@ void Cluster2D::classify(const float32_t * inferred_data) } int meta_type_id = 0; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] /= static_cast(obs->grids.size()); + obs->meta_type_probs[k] /= static_cast(obs->grids.size()); if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { meta_type_id = k; } @@ -325,7 +322,7 @@ tier4_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObjec } std::shared_ptr Cluster2D::getObjects( - const float32_t confidence_thresh, const float32_t height_thresh, const int32_t min_pts_num) + const float confidence_thresh, const float height_thresh, const int32_t min_pts_num) { auto object_array = std::make_shared(); diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp index 0575b633dd539..edf8276065163 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp @@ -12,17 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - namespace autoware { namespace perception @@ -31,16 +26,15 @@ namespace lidar_apollo_segmentation_tvm { namespace { -inline float32_t normalizeIntensity(float32_t intensity) +inline float normalizeIntensity(float intensity) { return intensity / 255; } } // namespace FeatureGenerator::FeatureGenerator( - const int32_t width, const int32_t height, const int32_t range, - const bool8_t use_intensity_feature, const bool8_t use_constant_feature, - const float32_t min_height, const float32_t max_height) + const int32_t width, const int32_t height, const int32_t range, const bool use_intensity_feature, + const bool use_constant_feature, const float min_height, const float max_height) : use_intensity_feature_(use_intensity_feature), use_constant_feature_(use_constant_feature), min_height_(min_height), @@ -62,13 +56,13 @@ FeatureGenerator::FeatureGenerator( std::shared_ptr FeatureGenerator::generate( const pcl::PointCloud::ConstPtr & pc_ptr) { - const float64_t epsilon = 1e-6; + const double epsilon = 1e-6; map_ptr_->resetMap(map_ptr_->map_data); const int32_t size = map_ptr_->height * map_ptr_->width; - const float32_t inv_res_x = 0.5f * map_ptr_->width / map_ptr_->range; - const float32_t inv_res_y = 0.5f * map_ptr_->height / map_ptr_->range; + const float inv_res_x = 0.5f * map_ptr_->width / map_ptr_->range; + const float inv_res_y = 0.5f * map_ptr_->height / map_ptr_->range; for (size_t i = 0; i < pc_ptr->points.size(); ++i) { if (pc_ptr->points[i].z <= min_height_ || max_height_ <= pc_ptr->points[i].z) { @@ -77,10 +71,10 @@ std::shared_ptr FeatureGenerator::generate( // x on grid const int32_t pos_x = static_cast( - std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].y) * inv_res_x)); + std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].y) * inv_res_x)); // y on grid const int32_t pos_y = static_cast( - std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].x) * inv_res_y)); + std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].x) * inv_res_y)); if (pos_x < 0 || map_ptr_->width <= pos_x || pos_y < 0 || map_ptr_->height <= pos_y) { continue; } @@ -101,7 +95,7 @@ std::shared_ptr FeatureGenerator::generate( } for (int32_t i = 0; i < size; ++i) { - if (static_cast(map_ptr_->count_data[i]) < epsilon) { + if (static_cast(map_ptr_->count_data[i]) < epsilon) { map_ptr_->max_height_data[i] = 0.0f; } else { map_ptr_->mean_height_data[i] /= map_ptr_->count_data[i]; diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp index 5533ba00a645c..a310ddae03f77 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include -using autoware::common::types::float32_t; - namespace autoware { namespace perception @@ -53,11 +50,11 @@ FeatureMap::FeatureMap(const int32_t width, const int32_t height, const int32_t count_data = &(map_data[0]) + width * height * 2; nonempty_data = &(map_data[0]) + width * height * 3; } -void FeatureMap::initializeMap(std::vector & map) +void FeatureMap::initializeMap(std::vector & map) { (void)map; } -void FeatureMap::resetMap(std::vector & map) +void FeatureMap::resetMap(std::vector & map) { const int32_t size = width * height; (void)map; @@ -80,11 +77,11 @@ FeatureMapWithIntensity::FeatureMapWithIntensity( mean_intensity_data = &(map_data[0]) + width * height * 4; nonempty_data = &(map_data[0]) + width * height * 5; } -void FeatureMapWithIntensity::initializeMap(std::vector & map) +void FeatureMapWithIntensity::initializeMap(std::vector & map) { (void)map; } -void FeatureMapWithIntensity::resetMap(std::vector & map) +void FeatureMapWithIntensity::resetMap(std::vector & map) { const int32_t size = width * height; (void)map; @@ -109,7 +106,7 @@ FeatureMapWithConstant::FeatureMapWithConstant( distance_data = &(map_data[0]) + width * height * 4; nonempty_data = &(map_data[0]) + width * height * 5; } -void FeatureMapWithConstant::initializeMap(std::vector & map) +void FeatureMapWithConstant::initializeMap(std::vector & map) { (void)map; for (int32_t row = 0; row < height; ++row) { @@ -119,16 +116,16 @@ void FeatureMapWithConstant::initializeMap(std::vector & map) // return the distance from the car to center of the grid. // Pc means point cloud = real world scale. so transform pixel scale to // real world scale - float32_t center_x = Pixel2Pc(row, height, range); - float32_t center_y = Pixel2Pc(col, width, range); + float center_x = Pixel2Pc(row, height, range); + float center_y = Pixel2Pc(col, width, range); // normalization. -0.5~0.5 - direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); + direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; } } } -void FeatureMapWithConstant::resetMap(std::vector & map) +void FeatureMapWithConstant::resetMap(std::vector & map) { const int32_t size = width * height; (void)map; @@ -153,7 +150,7 @@ FeatureMapWithConstantAndIntensity::FeatureMapWithConstantAndIntensity( distance_data = &(map_data[0]) + width * height * 6; nonempty_data = &(map_data[0]) + width * height * 7; } -void FeatureMapWithConstantAndIntensity::initializeMap(std::vector & map) +void FeatureMapWithConstantAndIntensity::initializeMap(std::vector & map) { (void)map; for (int32_t row = 0; row < height; ++row) { @@ -163,16 +160,16 @@ void FeatureMapWithConstantAndIntensity::initializeMap(std::vector & // return the distance from the car to center of the grid. // Pc means point cloud = real world scale. so transform pixel scale to // real world scale - float32_t center_x = Pixel2Pc(row, height, range); - float32_t center_y = Pixel2Pc(col, width, range); + float center_x = Pixel2Pc(row, height, range); + float center_y = Pixel2Pc(col, width, range); // normalization. -0.5~0.5 - direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); + direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; } } } -void FeatureMapWithConstantAndIntensity::resetMap(std::vector & map) +void FeatureMapWithConstantAndIntensity::resetMap(std::vector & map) { const int32_t size = width * height; (void)map; diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index f6f4531450d05..fb7ad38d7d642 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include #include #include @@ -22,8 +21,6 @@ #include #include -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; namespace autoware @@ -34,8 +31,7 @@ namespace lidar_apollo_segmentation_tvm { ApolloLidarSegmentationPreProcessor::ApolloLidarSegmentationPreProcessor( const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, - bool8_t use_intensity_feature, bool8_t use_constant_feature, float32_t min_height, - float32_t max_height) + bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height) : input_channels(config.network_inputs[0].node_shape[1]), input_width(config.network_inputs[0].node_shape[2]), input_height(config.network_inputs[0].node_shape[3]), @@ -75,9 +71,8 @@ TVMArrayContainerVector ApolloLidarSegmentationPreProcessor::schedule( ApolloLidarSegmentationPostProcessor::ApolloLidarSegmentationPostProcessor( const tvm_utility::pipeline::InferenceEngineTVMConfig & config, - const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, - float32_t objectness_thresh, float32_t score_threshold, float32_t height_thresh, - int32_t min_pts_num) + const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, float objectness_thresh, + float score_threshold, float height_thresh, int32_t min_pts_num) : output_channels(config.network_outputs[0].node_shape[1]), output_width(config.network_outputs[0].node_shape[2]), output_height(config.network_outputs[0].node_shape[3]), @@ -97,7 +92,7 @@ std::shared_ptr ApolloLidarSegmentationPostProcessor pcl::PointIndices valid_idx; valid_idx.indices.resize(pc_ptr_->size()); std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0); - std::vector feature(output_channels * output_width * output_height, 0); + std::vector feature(output_channels * output_width * output_height, 0); TVMArrayCopyToBytes( input[0].getArray(), feature.data(), output_channels * output_width * output_height * output_datatype_bytes); @@ -109,9 +104,9 @@ std::shared_ptr ApolloLidarSegmentationPostProcessor } ApolloLidarSegmentation::ApolloLidarSegmentation( - int32_t range, float32_t score_threshold, bool8_t use_intensity_feature, - bool8_t use_constant_feature, float32_t z_offset, float32_t min_height, float32_t max_height, - float32_t objectness_thresh, int32_t min_pts_num, float32_t height_thresh) + int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature, + float z_offset, float min_height, float max_height, float objectness_thresh, int32_t min_pts_num, + float height_thresh) : range_(range), score_threshold_(score_threshold), z_offset_(z_offset), @@ -132,7 +127,7 @@ ApolloLidarSegmentation::ApolloLidarSegmentation( void ApolloLidarSegmentation::transformCloud( const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, - float32_t z_offset) + float z_offset) { if (target_frame_ == input.header.frame_id && z_offset == 0) { transformed_cloud = input; @@ -149,8 +144,9 @@ void ApolloLidarSegmentation::transformCloud( transform_stamped = tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl::transformPointCloud(in_cluster, transformed_cloud_cluster, affine_matrix); + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + tier4_autoware_utils::transformPointCloud( + in_cluster, transformed_cloud_cluster, affine_matrix); transformed_cloud_cluster.header.frame_id = target_frame_; } else { transformed_cloud_cluster = in_cluster; @@ -160,7 +156,7 @@ void ApolloLidarSegmentation::transformCloud( if (z_offset != 0) { Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - pcl::transformPointCloud( + tier4_autoware_utils::transformPointCloud( transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); } diff --git a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp index 3389f753364bf..a4b967de2e9bc 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include -using autoware::common::types::float32_t; - namespace autoware { namespace perception @@ -29,19 +26,19 @@ namespace lidar_apollo_segmentation_tvm { struct LogTable { - std::vector data; + std::vector data; LogTable() { data.resize(256 * 10); for (size_t i = 0; i < data.size(); ++i) { - data[i] = std::log1p(static_cast(i) / 10); + data[i] = std::log1p(static_cast(i) / 10); } } }; static LogTable log_table; -float32_t calcApproximateLog(float32_t num) +float calcApproximateLog(float num) { if (num >= 0) { size_t integer_num = static_cast(num * 10.0f); diff --git a/perception/lidar_apollo_segmentation_tvm/test/main.cpp b/perception/lidar_apollo_segmentation_tvm/test/main.cpp index 40907845b49ad..9fd644afff1f2 100644 --- a/perception/lidar_apollo_segmentation_tvm/test/main.cpp +++ b/perception/lidar_apollo_segmentation_tvm/test/main.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -23,8 +22,6 @@ #include #include -using autoware::common::types::float32_t; -using autoware::common::types::uchar8_t; using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bool expect_throw) @@ -35,11 +32,11 @@ void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bo const int range = 70; const float score_threshold = 0.8f; const float z_offset = 1.0f; - const float32_t min_height = -5.0f; - const float32_t max_height = 5.0f; - const float32_t objectness_thresh = 0.5f; + const float min_height = -5.0f; + const float max_height = 5.0f; + const float objectness_thresh = 0.5f; const int32_t min_pts_num = 3; - const float32_t height_thresh = 0.5f; + const float height_thresh = 0.5f; ApolloLidarSegmentation segmentation( range, score_threshold, use_intensity_feature, use_constant_feature, z_offset, min_height, max_height, objectness_thresh, min_pts_num, height_thresh); @@ -49,10 +46,10 @@ void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bo std::random_device rd; std::mt19937 gen(42); - std::uniform_real_distribution dis(-50.0, 50.0); - std::vector v(width * height * sizeof(float32_t) * 4); + std::uniform_real_distribution dis(-50.0, 50.0); + std::vector v(width * height * sizeof(float) * 4); for (size_t i = 0; i < width * height * 4; i++) { - reinterpret_cast(v.data())[i] = dis(gen); + reinterpret_cast(v.data())[i] = dis(gen); } sensor_msgs::msg::PointCloud2 input{}; @@ -63,10 +60,10 @@ void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bo input.fields[2U].name = "z"; input.fields[3U].name = "intensity"; for (uint32_t idx = 0U; idx < 4U; ++idx) { - input.fields[idx].offset = static_cast(idx * sizeof(float32_t)); + input.fields[idx].offset = static_cast(idx * sizeof(float)); input.fields[idx].datatype = sensor_msgs::msg::PointField::FLOAT32; input.fields[idx].count = 1U; - input.point_step += static_cast(sizeof(float32_t)); + input.point_step += static_cast(sizeof(float)); } input.height = static_cast(height); input.width = static_cast(width); diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt index f42ee8e97c08e..100e14277ec1b 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt +++ b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright 2021-2022 Arm Ltd. +# Copyright 2021-2023 Arm Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -44,7 +44,7 @@ if(lidar_apollo_segmentation_tvm_BUILT) ament_auto_package(INSTALL_TO_SHARE launch - param + config ) else() message(WARNING "lidar_apollo_segmentation_tvm not built, skipping package.") diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/README.md b/perception/lidar_apollo_segmentation_tvm_nodes/README.md index e44be0a7ec504..626048b2416a0 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/README.md +++ b/perception/lidar_apollo_segmentation_tvm_nodes/README.md @@ -1,3 +1,19 @@ + + # lidar_apollo_segmentation_tvm_nodes ## Purpose / Use cases @@ -33,21 +49,6 @@ The input are non-ground points as a PointCloud2 message from the sensor_msgs pa The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg). -#### Parameters - -| Parameter | Type | Description | Default | -| ----------------------- | ------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- | -| `range` | _int_ | The range of the 2D grid with respect to the origin (the LiDAR sensor). Unit: meter | `70` | -| `score_threshold` | _float_ | The detection confidence score threshold for filtering out the candidate clusters in the post-processing step. | `0.8` | -| `use_intensity_feature` | _bool_ | Enable input channel intensity feature. | `true` | -| `use_constant_feature` | _bool_ | Enable input channel constant feature. | `false` | -| `z_offset` | _float_ | The offset to translate up the input pointcloud before the inference. Unit: meter | `0.0` | -| `min_height` | _float_ | The minimum height with respect to the origin (the LiDAR sensor). Unit: meter | `-5.0` | -| `max_height` | _float_ | The maximum height with respect to the origin (the LiDAR sensor). Unit: meter | `5.0` | -| `objectness_thresh` | _float_ | The threshold of objectness for filtering out non-object cells in the obstacle clustering step. | `0.5` | -| `min_pts_num` | _int_ | In the post-processing step, the candidate clusters with less than min_pts_num points are removed. | `3` | -| `height_thresh` | _float_ | If it is non-negative, the points that are higher than the predicted object height by height_thresh are filtered out in the post-processing step. Unit: meter | `0.5` | - ### Error detection and handling Abort and warn when the input frame can't be converted to `base_link`. diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml b/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml new file mode 100644 index 0000000000000..97f733f01c794 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml @@ -0,0 +1,26 @@ +# Copyright 2023 Arm Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +/**: + ros__parameters: + range: 90 + score_threshold: 0.1 + use_intensity_feature: false + use_constant_feature: false + z_offset: 0.0 + min_height: -5.0 + max_height: 5.0 + objectness_thresh: 0.5 + min_pts_num: 3 + height_thresh: 0.5 diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py index 6abf95dce7789..f4c81d6b154d7 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py +++ b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py @@ -1,4 +1,4 @@ -# Copyright 2021-2022 the Autoware Foundation +# Copyright 2021-2023 Arm Ltd., the Autoware Foundation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -26,7 +26,8 @@ def generate_launch_description(): param_file = os.path.join( - get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), "param/test.param.yaml" + get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), + "config/lidar_apollo_segmentation_tvm_nodes.param.yaml", ) with open(param_file, "r") as f: lidar_apollo_segmentation_tvm_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml index 624e25c7d4ba6..0d7bebe986df4 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml +++ b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml @@ -1,7 +1,22 @@ + - + diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml b/perception/lidar_apollo_segmentation_tvm_nodes/package.xml index 77211b7899878..b6c6cb261eb29 100755 --- a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml +++ b/perception/lidar_apollo_segmentation_tvm_nodes/package.xml @@ -10,10 +10,8 @@ ament_cmake ament_cmake_auto + autoware_cmake - autoware_cmake - - autoware_auto_common lidar_apollo_segmentation_tvm rclcpp rclcpp_components diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml b/perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml deleted file mode 100644 index 841148a8e941f..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml +++ /dev/null @@ -1,13 +0,0 @@ -/**: - ros__parameters: - # The range of the 2D grid with respect to the origin. - range: 90 - # The detection confidence score threshold for filtering out the candidate - # clusters in the post-processing step. - score_threshold: 0.1 - # Enable input channel intensity feature. - use_intensity_feature: false - # Enable input channel constant feature. - use_constant_feature: false - # Vertical translation of the pointcloud before inference. - z_offset: 0.0 diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json b/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json new file mode 100644 index 0000000000000..8e56cfb820766 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json @@ -0,0 +1,92 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lidar Apollo Segmentation TVM Nodes", + "type": "object", + "definitions": { + "lidar_apollo_segmentation_tvm_nodes": { + "type": "object", + "properties": { + "range": { + "type": "integer", + "default": 90, + "exclusiveMinimum": 0, + "description": "The range of the 2D grid with respect to the origin." + }, + "score_threshold": { + "type": "number", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0, + "description": "The detection confidence score threshold for filtering out the candidate clusters in the post-processing step." + }, + "use_intensity_feature": { + "type": "boolean", + "default": "false", + "description": "Enable input channel intensity feature." + }, + "use_constant_feature": { + "type": "boolean", + "default": "false", + "description": "Enable input channel constant feature." + }, + "z_offset": { + "type": "number", + "default": 0.0, + "description": "Vertical translation of the pointcloud before inference." + }, + "min_height": { + "type": "number", + "default": -5.0, + "description": "The minimum height with respect to the origin" + }, + "max_height": { + "type": "number", + "default": 5.0, + "description": "The maximum height with respect to the origin." + }, + "objectness_thresh": { + "type": "number", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0, + "description": "The threshold of objectness for filtering out non-object cells in the obstacle clustering step." + }, + "min_pts_num": { + "type": "integer", + "default": 3, + "minimum": 0, + "description": "In the post-processing step, the candidate clusters with less than min_pts_num points are removed." + }, + "height_thresh": { + "type": "number", + "default": 0.5, + "description": "If it is non-negative, the points that are higher than the predicted object height by height_thresh are filtered out in the post-processing step." + } + }, + "required": [ + "range", + "score_threshold", + "use_intensity_feature", + "use_constant_feature", + "z_offset", + "min_height", + "max_height", + "objectness_thresh", + "min_pts_num", + "height_thresh" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lidar_apollo_segmentation_tvm_nodes" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp b/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp index d96eaf9cf084c..8f1d3301cf3a2 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp +++ b/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020-2022 Arm Ltd., TierIV +// Copyright 2020-2023 Arm Ltd., TierIV // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -20,9 +19,6 @@ #include -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; namespace autoware @@ -40,16 +36,12 @@ ApolloLidarSegmentationNode::ApolloLidarSegmentationNode(const rclcpp::NodeOptio m_detected_pub_ptr{create_publisher( "objects_out", rclcpp::QoS{1})}, m_detector_ptr{std::make_shared( - declare_parameter("range", rclcpp::ParameterValue{70}).get(), - declare_parameter("score_threshold", rclcpp::ParameterValue{0.8}).get(), - declare_parameter("use_intensity_feature", rclcpp::ParameterValue{true}).get(), - declare_parameter("use_constant_feature", rclcpp::ParameterValue{false}).get(), - declare_parameter("z_offset", rclcpp::ParameterValue{0.0}).get(), - declare_parameter("min_height", rclcpp::ParameterValue{-5.0}).get(), - declare_parameter("max_height", rclcpp::ParameterValue{5.0}).get(), - declare_parameter("objectness_thresh", rclcpp::ParameterValue{0.5}).get(), - declare_parameter("min_pts_num", rclcpp::ParameterValue{3}).get(), - declare_parameter("height_thresh", rclcpp::ParameterValue{0.5}).get())} + declare_parameter("range"), declare_parameter("score_threshold"), + declare_parameter("use_intensity_feature"), + declare_parameter("use_constant_feature"), declare_parameter("z_offset"), + declare_parameter("min_height"), declare_parameter("max_height"), + declare_parameter("objectness_thresh"), declare_parameter("min_pts_num"), + declare_parameter("height_thresh"))} { // Log unexpected versions of the neural network. auto version_status = m_detector_ptr->version_check(); diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index b1cb925f5e49f..f5a15916f31a9 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -56,8 +56,8 @@ You can download the onnx format of trained models by clicking on the links belo - Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx) - Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx) -`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. -`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. +`Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. +`Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. ## Standalone inference and visualization diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md index 69ef70be265ec..6a743b6e9d9c1 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md @@ -124,13 +124,13 @@ ros2 launch lidar_centerpoint centerpoint_vs_centerpoint-tiny.launch.xml Then you will see two rviz window show immediately. On the left is the result for lidar centerpoint tiny, and on the right is the result for lidar centerpoint. -![two rviz2 display centerpoint and centerpoint_tiny](https://i.imgur.com/YAYehrf.jpg) +![two rviz2 display centerpoint and centerpoint_tiny](https://github.com/autowarefoundation/autoware.universe/assets/58775300/2a89063a-8e0e-4f59-8d48-f339d4f7c0ff) ## Troubleshooting ### Bounding Box blink on rviz -To avoid Bounding Boxs blinking on rviz, you can extend bbox marker lifetime. +To avoid Bounding Boxes blinking on rviz, you can extend bbox marker lifetime. Set `marker_ptr->lifetime` and `marker.lifetime` to a longer lifetime. diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 879013189b246..49f51a648dcf1 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -10,8 +10,7 @@ ament_cmake_auto ament_cmake_python - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs pcl_ros diff --git a/perception/lidar_centerpoint_tvm/CMakeLists.txt b/perception/lidar_centerpoint_tvm/CMakeLists.txt index 744450d5733b1..e9da98d4ae3c4 100644 --- a/perception/lidar_centerpoint_tvm/CMakeLists.txt +++ b/perception/lidar_centerpoint_tvm/CMakeLists.txt @@ -68,6 +68,28 @@ if((NOT NN_DEPENDENCY_ENCODER STREQUAL "") AND (NOT NN_DEPENDENCY_BACKBONE STREQ EXECUTABLE lidar_centerpoint_tvm_node ) + ## single inference node ## + ament_auto_add_library(single_inference_lidar_centerpoint_tvm_component SHARED + src/single_inference_node.cpp + ) + + target_link_libraries(single_inference_lidar_centerpoint_tvm_component + ${tvm_runtime_LIBRARIES} + ) + + + rclcpp_components_register_node(single_inference_lidar_centerpoint_tvm_component + PLUGIN "autoware::perception::lidar_centerpoint_tvm::SingleInferenceLidarCenterPointNode" + EXECUTABLE single_inference_lidar_centerpoint_tvm_node + ) + + install(PROGRAMS + scripts/lidar_centerpoint_visualizer.py + DESTINATION lib/${PROJECT_NAME} + ) + + ament_export_dependencies(ament_cmake_python) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/lidar_centerpoint_tvm/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint_tvm/config/centerpoint_tiny.param.yaml deleted file mode 100644 index 0b23c48887b6f..0000000000000 --- a/perception/lidar_centerpoint_tvm/config/centerpoint_tiny.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true - point_feature_size: 4 - max_voxel_size: 40000 - point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] - voxel_size: [0.32, 0.32, 8.0] - downsample_factor: 2 - encoder_in_feature_size: 9 diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp new file mode 100644 index 0000000000000..4c3a7a2c9ecf3 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_CENTERPOINT_TVM__SINGLE_INFERENCE_NODE_HPP_ +#define LIDAR_CENTERPOINT_TVM__SINGLE_INFERENCE_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_centerpoint_tvm +{ +class SingleInferenceLidarCenterPointNode : public rclcpp::Node +{ +public: + explicit SingleInferenceLidarCenterPointNode(const rclcpp::NodeOptions & node_options); + +private: + void detect(const std::string & pcd_path, const std::string & detections_path); + std::vector getVertices( + const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const; + void dumpDetectionsAsMesh( + const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, + const std::string & output_path) const; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + float score_threshold_{0.0}; + std::vector class_names_; + bool rename_car_to_truck_and_bus_{false}; + bool has_twist_{false}; + + std::unique_ptr detector_ptr_{nullptr}; +}; + +} // namespace lidar_centerpoint_tvm +} // namespace perception +} // namespace autoware + +#endif // LIDAR_CENTERPOINT_TVM__SINGLE_INFERENCE_NODE_HPP_ diff --git a/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml b/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml index dbb4b32e44404..e838f01e347ce 100644 --- a/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml +++ b/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml b/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml new file mode 100644 index 0000000000000..e62808804ca07 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp index 299a7eeb98044..5562e963d177b 100644 --- a/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp @@ -45,7 +45,7 @@ boost::optional getTransform( target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); return transform_stamped.transform; } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), ex.what()); + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint_tvm"), ex.what()); return boost::none; } } @@ -77,14 +77,18 @@ bool PointCloudDensification::enqueuePointCloud( { const auto header = pointcloud_msg.header; - auto transform_world2current = - getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); - if (!transform_world2current) { - return false; + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); } - auto affine_world2current = transformToEigen(transform_world2current.get()); - - enqueue(pointcloud_msg, affine_world2current); dequeue(); return true; diff --git a/perception/lidar_centerpoint_tvm/package.xml b/perception/lidar_centerpoint_tvm/package.xml index 22c4bd65ee653..058db088ef332 100644 --- a/perception/lidar_centerpoint_tvm/package.xml +++ b/perception/lidar_centerpoint_tvm/package.xml @@ -9,8 +9,7 @@ Apache 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs pcl_ros diff --git a/perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py new file mode 100755 index 0000000000000..5a1135379615f --- /dev/null +++ b/perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python +# Copyright 2022 TIER IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import time + +import open3d as o3d +import rclpy +from rclpy.node import Node + + +def main(args=None): + rclpy.init(args=args) + + node = Node("lidar_centerpoint_visualizer") + node.declare_parameter("pcd_path", rclpy.Parameter.Type.STRING) + node.declare_parameter("detections_path", rclpy.Parameter.Type.STRING) + + pcd_path = node.get_parameter("pcd_path").get_parameter_value().string_value + detections_path = node.get_parameter("detections_path").get_parameter_value().string_value + + while not os.path.exists(pcd_path) and not os.path.exists(detections_path): + time.sleep(1.0) + + if not rclpy.ok(): + rclpy.shutdown() + return + + mesh = o3d.io.read_triangle_mesh(detections_path) + pcd = o3d.io.read_point_cloud(pcd_path) + + mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) + + detection_lines = o3d.geometry.LineSet.create_from_triangle_mesh(mesh) + detection_lines.paint_uniform_color([1.0, 0.0, 1.0]) + + o3d.visualization.draw_geometries([mesh_frame, pcd, detection_lines]) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp b/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp new file mode 100644 index 0000000000000..8b810c5a7d759 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp @@ -0,0 +1,239 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_centerpoint_tvm/single_inference_node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_centerpoint_tvm +{ + +SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( + const rclcpp::NodeOptions & node_options) +: Node("lidar_center_point_tvm", node_options), tf_buffer_(this->get_clock()) +{ + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", 0.35)); + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + const float yaw_norm_threshold = + static_cast(this->declare_parameter("yaw_norm_threshold", 0.0)); + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", "map"); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", 0); + + class_names_ = this->declare_parameter>("class_names"); + has_twist_ = this->declare_parameter("has_twist", false); + const std::size_t point_feature_size = + static_cast(this->declare_parameter("point_feature_size")); + const std::size_t max_voxel_size = + static_cast(this->declare_parameter("max_voxel_size")); + const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); + const auto voxel_size = this->declare_parameter>("voxel_size"); + const std::size_t downsample_factor = + static_cast(this->declare_parameter("downsample_factor")); + const std::size_t encoder_in_feature_size = + static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto pcd_path = this->declare_parameter("pcd_path"); + const auto detections_path = this->declare_parameter("detections_path"); + + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The size of voxel_size != 3: use the default parameters."); + } + CenterPointConfig config( + class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_threshold); + detector_ptr_ = std::make_unique(densification_param, config); + + detect(pcd_path, detections_path); + exit(0); +} + +std::vector SingleInferenceLidarCenterPointNode::getVertices( + const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const +{ + std::vector vertices; + Eigen::Vector3d vertex; + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + return vertices; +} + +void SingleInferenceLidarCenterPointNode::detect( + const std::string & pcd_path, const std::string & detections_path) +{ + sensor_msgs::msg::PointCloud2 msg; + pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud()); + + pcl::io::loadPCDFile(pcd_path, *pc_ptr); + pcl::toROSMsg(*pc_ptr, msg); + msg.header.frame_id = "lidar_frame"; + + std::vector det_boxes3d; + bool is_success = detector_ptr_->detect(msg, tf_buffer_, det_boxes3d); + if (!is_success) { + return; + } + + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = msg.header; + for (const auto & box3d : det_boxes3d) { + autoware_auto_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + output_msg.objects.emplace_back(obj); + } + + dumpDetectionsAsMesh(output_msg, detections_path); + + RCLCPP_INFO( + rclcpp::get_logger("single_inference_lidar_centerpoint_tvm"), + "The detection results were saved as meshes in %s", detections_path.c_str()); +} + +void SingleInferenceLidarCenterPointNode::dumpDetectionsAsMesh( + const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, + const std::string & output_path) const +{ + std::ofstream ofs(output_path, std::ofstream::out); + std::stringstream vertices_stream; + std::stringstream faces_stream; + int index = 0; + int num_detections = static_cast(objects_msg.objects.size()); + + ofs << "ply" << std::endl; + ofs << "format ascii 1.0" << std::endl; + ofs << "comment created by lidar_centerpoint" << std::endl; + ofs << "element vertex " << 8 * num_detections << std::endl; + ofs << "property float x" << std::endl; + ofs << "property float y" << std::endl; + ofs << "property float z" << std::endl; + ofs << "element face " << 12 * num_detections << std::endl; + ofs << "property list uchar uint vertex_indices" << std::endl; + ofs << "end_header" << std::endl; + + auto streamFace = [&faces_stream](int v1, int v2, int v3) { + faces_stream << std::to_string(3) << " " << std::to_string(v1) << " " << std::to_string(v2) + << " " << std::to_string(v3) << std::endl; + }; + + for (const auto & object : objects_msg.objects) { + Eigen::Affine3d pose_affine; + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, pose_affine); + + std::vector vertices = getVertices(object.shape, pose_affine); + + for (const auto & vertex : vertices) { + vertices_stream << vertex.x() << " " << vertex.y() << " " << vertex.z() << std::endl; + } + + streamFace(index + 1, index + 3, index + 4); + streamFace(index + 3, index + 5, index + 6); + streamFace(index + 0, index + 7, index + 5); + streamFace(index + 7, index + 2, index + 4); + streamFace(index + 5, index + 3, index + 1); + streamFace(index + 7, index + 0, index + 2); + streamFace(index + 2, index + 1, index + 4); + streamFace(index + 4, index + 3, index + 6); + streamFace(index + 5, index + 7, index + 6); + streamFace(index + 6, index + 7, index + 4); + streamFace(index + 0, index + 5, index + 1); + index += 8; + } + + ofs << vertices_stream.str(); + ofs << faces_stream.str(); + + ofs.close(); +} + +} // namespace lidar_centerpoint_tvm +} // namespace perception +} // namespace autoware + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::perception::lidar_centerpoint_tvm::SingleInferenceLidarCenterPointNode) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 043103e9c39d7..256a1d0ae899f 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -38,51 +38,76 @@ Search one or more lanelets satisfying the following conditions for each target #### Get predicted reference path -- Get reference path +- Get reference path: - Create a reference path for the object from the associated lanelet. -- Predict Object Maneuver +- Predict object maneuver: - Generate predicted paths for the object. - - The probability is assigned to each maneuver of `Lane Follow`, `Left Lane Change`, and `Right Lane Change` based on the object history and the reference path obtained in the first step. - - The following information is used to determine the maneuver. - - The distance between the current center of gravity of the object and the left and right boundaries of the lane - - The lateral velocity (distance moved to the lateral direction in `t` seconds) + - Assign probability to each maneuver of `Lane Follow`, `Left Lane Change`, and `Right Lane Change` based on the object history and the reference path obtained in the first step. + - Lane change decision is based on two domains: + - Geometric domain: the lateral distance between the center of gravity of the object and left/right boundaries of the lane. + - Time domain: estimated time margin for the object to reach the left/right bound. -The conditions for the lane change detection then becomes +The conditions for left lane change detection are: -```cpp -// Left Lane Change Detection -(d_current_left / d_lane) > dl_ratio_threshold && -(d_current_left - d_previous_left) > ddl_threshold +- Check if the distance to the left lane boundary is less than the distance to the right lane boundary. +- Check if the distance to the left lane boundary is less than a `dist_threshold_to_bound_`. +- Check if the lateral velocity direction is towards the left lane boundary. +- Check if the time to reach the left lane boundary is less than `time_threshold_to_bound_`. -// Right Lane Change Detection -(d_current_right / d_lane) < dr_ratio_threshold && -(d_current_right - d_previous_right) < ddr_threshold -``` - -where the parameter is explained in the picture below. An example of how to tune the parameters is described later. +Lane change logics is illustrated in the figure below.An example of how to tune the parameters is described later. ![lane change detection](./media/lane_change_detection.drawio.svg) -- Calculate Object Probability +- Calculate object probability: - The path probability obtained above is calculated based on the current position and angle of the object. -- Refine predicted paths for smooth movement +- Refine predicted paths for smooth movement: - The generated predicted paths are recomputed to take the vehicle dynamics into account. - - The path is calculated with minimum jerk trajectory implemented by 4th/5th order spline for lateral/longitudinal motion.引く。 + - The path is calculated with minimum jerk trajectory implemented by 4th/5th order spline for lateral/longitudinal motion. + +### Tuning lane change detection logic + +Currently we provide two 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 +- `cutoff_freq_of_velocity_lpf_`: cutoff frequency of low pass filter for lateral velocity + +You can change these parameters in rosparam in the table below. + +| param name | default value | +| --------------------------------------------------- | ------------- | +| `dist_threshold_for_lane_change_detection` | `1.0` [m] | +| `time_threshold_for_lane_change_detection` | `5.0` [s] | +| `cutoff_freq_of_velocity_for_lane_change_detection` | `0.1` [Hz] | + +#### Tuning threshold parameters + +Increasing these two parameters will slow down and stabilize the lane change estimation. + +Normally, we recommend tuning only `time_threshold_for_lane_change_detection` because it is the more important factor for lane change decision. + +#### Tuning lateral velocity calculation + +Lateral velocity calculation is also a very important factor for lane change decision because it is used in the time domain decision. -### Lane change detection logic +The predicted time to reach the lane boundary is calculated by -This is an example to tune the parameters for lane change detection. -The default parameters are set so that the lane change can be detected 1 second before the vehicle crosses the lane boundary. Here, 15 data in the lane change / non lane change cases are plotted. +$$ +t_{predicted} = \dfrac{d_{lat}}{v_{lat}} +$$ -![right change data](./media/lanechange_detection_right_to_left.png) +where $d_{lat}$ and $v_{lat}$ represent the lateral distance to the lane boundary and the lateral velocity, respectively. -On the top plot, blue dots are the distance from the lane boundary one second before the lane change, and red dots are the average distance from the lane boundary when driving straight. From this plot, the most conservative value where lane change can be detected for all of these data can be seen as `-1.1`. Note that the larger number makes the decision conservative (lane change may not be detected) and the lower number makes the decision aggressive (many false positive occurs). +Lowering the cutoff frequency of the low-pass filter for lateral velocity will make the lane change decision more stable but slower. Our setting is very conservative, so you may increase this parameter if you want to make the lane change decision faster. -On the bottom plot, blue dots are the lateral velocity one second before the lane change, and red dots are the average of the (absolute) lateral velocity when driving straight. In the same policy above, the parameter can be set as `0.5`. +For the additional information, here we show how we calculate lateral velocity. -#### Limitations +| lateral velocity calculation method | equation | description | +| ------------------------------------------------------------- | ---------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| [**applied**] time derivative of lateral distance | $\dfrac{\Delta d_{lat}}{\Delta t}$ | Currently, we use this method to deal with winding roads. Since this time differentiation easily becomes noisy, we also use a low-pass filter to get smoothed velocity. | +| [not applied] Object Velocity Projection to Lateral Direction | $v_{obj} \sin(\theta)$ | Normally, object velocities are less noisy than the time derivative of lateral distance. But the yaw difference $\theta$ between the lane and object directions sometimes becomes discontinuous, so we did not adopt this method. | -- This plot shows only for one environment data. The parameter/algorithm must consider lane width. (The default parameters are set roughly considering the generalization of the lane width for other environments.) +Currently, we use the upper method with a low-pass filter to calculate lateral velocity. ### Path prediction for crosswalk users 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 ab2519876f8a6..6bc1315a47e72 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -11,7 +11,17 @@ sigma_yaw_angle_deg: 5.0 #[angle degree] object_buffer_time_length: 2.0 #[s] history_time_length: 1.0 #[s] - dist_threshold_for_lane_change_detection: 1.0 #[m] - time_threshold_for_lane_change_detection: 5.0 #[s] - cutoff_freq_of_velocity_for_lane_change_detection: 0.1 #[Hz] + + lane_change_detection: + method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane" + time_to_change_lane: + dist_threshold_for_lane_change_detection: 1.0 #[m] + time_threshold_for_lane_change_detection: 5.0 #[s] + cutoff_freq_of_velocity_for_lane_change_detection: 0.1 #[Hz] + lat_diff_distance: + dist_ratio_threshold_to_left_bound: -0.5 #[ratio] + dist_ratio_threshold_to_right_bound: 0.5 #[ratio + diff_dist_threshold_to_left_bound: 0.29 #[m] + diff_dist_threshold_to_right_bound: -0.29 #[m] + reference_path_resolution: 0.5 #[m] 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 0879c0a213501..4ac5fd76babea 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 @@ -145,9 +145,14 @@ class MapBasedPredictionNode : public rclcpp::Node double sigma_yaw_angle_deg_; double object_buffer_time_length_; double history_time_length_; + std::string lane_change_detection_method_; double dist_threshold_to_bound_; double time_threshold_to_bound_; double cutoff_freq_of_velocity_lpf_; + double dist_ratio_threshold_to_left_bound_; + double dist_ratio_threshold_to_right_bound_; + double diff_dist_threshold_to_left_bound_; + double diff_dist_threshold_to_right_bound_; double reference_path_resolution_; // Stop watch @@ -182,7 +187,7 @@ class MapBasedPredictionNode : public rclcpp::Node const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time); Maneuver predictObjectManeuver( - const TrackedObject & object, const LaneletData & current_lanelet, + const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); geometry_msgs::msg::Pose compensateTimeDelay( const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist, @@ -213,6 +218,13 @@ class MapBasedPredictionNode : public rclcpp::Node visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); + + Maneuver predictObjectManeuverByTimeToLaneChange( + const TrackedObject & object, const LaneletData & current_lanelet_data, + const double object_detected_time); + Maneuver predictObjectManeuverByLatDiffDistance( + const TrackedObject & object, const LaneletData & current_lanelet_data, + const double object_detected_time); }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/media/lane_change_detection.drawio.svg b/perception/map_based_prediction/media/lane_change_detection.drawio.svg index ac741c76dfcd2..565ce669baeb4 100644 --- a/perception/map_based_prediction/media/lane_change_detection.drawio.svg +++ b/perception/map_based_prediction/media/lane_change_detection.drawio.svg @@ -1,165 +1,567 @@ - - - - - - - + + + + + + + + + + +
+
+
+ left_dist +
+
+
+ + left_dist + + + + + +
+
+
+ right_dist +
+
+
+ + right_dist + + + + + + + + + + + + + + + + + +
+
+
+ right_dist +
+
+
+ + right_dist + + + + + +
+
+
+ left_dist +
+
+
+ + left_dist + + + + + +
+
+
+ v_left_filtered +
+
+
+ + v_left_fil... + + + + + + + + + + + + + +
+
+
time
+
+
+ + time + + - - - - - - -
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t=t_n` + + + + + +
-
- - d_lane - +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- d_lane + `t=t_{n-1}` - - - - - - - -
+
-
- - d_previous, left - +
+ time_margin_to_left_boundary
- d_previous, left + time_margin_to_left_boundary - + + + + + -
-
-
- - d_previous, right - +
+
+
+ distance from boundary
- d_previous, right + distance f... - + + + -
-
-
- - t-1 - +
+
+
+ geometric
- t-1 + geometric - + -
-
-
- - t - +
+
+
+ time domain
- t + time domain - - - - - - - -
-
-
- - d_current, right - +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t=t_{n-1}` + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- d_current,... + `t=t_n` - -
+
-
- - d_current, left - +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- d_current, left + `t_{predict}` diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 74742179a1577..33abfe814c498 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -7,11 +7,11 @@ Yutaka Shimizu Tomoya Kimura Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs interpolation 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 fe0686766a569..dd18083af88b9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -188,6 +188,25 @@ void updateLateralKinematicsVector( } } +/** + * @brief calc absolute normalized yaw difference between lanelet and object + * + * @param object + * @param lanelet + * @return double + */ +double calcAbsYawDiffBetweenLaneletAndObject( + const TrackedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = + lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + return abs_norm_delta; +} + lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data) { lanelet::ConstLanelets lanelets; @@ -213,7 +232,9 @@ EntryPoint getCrosswalkEntryPoint(const lanelet::ConstLanelet & crosswalk) return std::make_pair(front_entry_point, back_entry_point); } -bool withinLanelet(const TrackedObject & object, const lanelet::ConstLanelet & lanelet) +bool withinLanelet( + const TrackedObject & object, const lanelet::ConstLanelet & lanelet, + const bool use_yaw_information = false, const float yaw_threshold = 0.6) { using Point = boost::geometry::model::d2::point_xy; @@ -222,11 +243,24 @@ bool withinLanelet(const TrackedObject & object, const lanelet::ConstLanelet & l auto polygon = lanelet.polygon2d().basicPolygon(); boost::geometry::correct(polygon); + bool with_in_polygon = boost::geometry::within(p_object, polygon); - return boost::geometry::within(p_object, polygon); + if (!use_yaw_information) { + return with_in_polygon; + } else { + // use yaw angle to compare + const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); + if (abs_yaw_diff < yaw_threshold) { + return with_in_polygon; + } else { + return false; + } + } } -bool withinRoadLanelet(const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr) +bool withinRoadLanelet( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const bool use_yaw_information = false) { using Point = boost::geometry::model::d2::point_xy; @@ -249,7 +283,7 @@ bool withinRoadLanelet(const TrackedObject & object, const lanelet::LaneletMapPt } } - if (withinLanelet(object, lanelet.second)) { + if (withinLanelet(object, lanelet.second, use_yaw_information)) { return true; } } @@ -387,6 +421,62 @@ bool hasPotentialToReach( return false; } +/** + * @brief change label for prediction + * + * @param label + * @return ObjectClassification::_label_type + */ +ObjectClassification::_label_type changeLabelForPrediction( + const ObjectClassification::_label_type & label, const TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr_) +{ + // for car like vehicle do not change labels + if ( + label == ObjectClassification::CAR || label == ObjectClassification::BUS || + label == ObjectClassification::TRUCK || label == ObjectClassification::TRAILER || + label == ObjectClassification::UNKNOWN) { + return label; + } else if ( // for bicycle and motorcycle + label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE) { + // if object is within road lanelet and satisfies yaw constraints + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bycicle 25 km/h + const bool high_speed_object = + object.kinematics.twist_with_covariance.twist.linear.x > high_speed_threshold; + + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) { + return ObjectClassification::MOTORCYCLE; + } else if (high_speed_object) { + // high speed object outside road lanelet will move like unknown object + // return ObjectClassification::UNKNOWN; // temporary disabled + return label; + } else { + return ObjectClassification::BICYCLE; + } + } else if (label == ObjectClassification::PEDESTRIAN) { + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float max_velocity_for_human_mps = + 25.0 / 18.0 * 5.0; // Max human being motion speed is 25km/h + const bool high_speed_object = + object.kinematics.twist_with_covariance.twist.linear.x > max_velocity_for_human_mps; + // fast, human-like object: like segway + if (within_road_lanelet && high_speed_object) { + return label; // currently do nothing + // return ObjectClassification::MOTORCYCLE; + } else if (high_speed_object) { + return label; // currently do nothing + // fast human outside road lanelet will move like unknown object + // return ObjectClassification::UNKNOWN; + } else { + return label; + } + } else { + return label; + } +} + MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) { @@ -404,12 +494,30 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg", 5.0); object_buffer_time_length_ = declare_parameter("object_buffer_time_length", 2.0); history_time_length_ = declare_parameter("history_time_length", 1.0); - dist_threshold_to_bound_ = - declare_parameter("dist_threshold_for_lane_change_detection", 1.0); // 1m - time_threshold_to_bound_ = - declare_parameter("time_threshold_for_lane_change_detection", 5.0); // 5 sec - cutoff_freq_of_velocity_lpf_ = - declare_parameter("cutoff_freq_of_velocity_for_lane_change_detection", 0.1); // 0.1Hz + { // lane change detection + lane_change_detection_method_ = declare_parameter("lane_change_detection.method"); + + // lane change detection by time_to_change_lane + dist_threshold_to_bound_ = declare_parameter( + "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection", + 1.0); // 1m + time_threshold_to_bound_ = declare_parameter( + "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection", + 5.0); // 5 sec + cutoff_freq_of_velocity_lpf_ = declare_parameter( + "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_detection", + 0.1); // 0.1Hz + + // lane change detection by lat_diff_distance + dist_ratio_threshold_to_left_bound_ = declare_parameter( + "lane_change_detection.lat_diff_distance.dist_ratio_threshold_to_left_bound"); + dist_ratio_threshold_to_right_bound_ = declare_parameter( + "lane_change_detection.lat_diff_distance.dist_ratio_threshold_to_right_bound"); + diff_dist_threshold_to_left_bound_ = declare_parameter( + "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_left_bound"); + diff_dist_threshold_to_right_bound_ = declare_parameter( + "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound"); + } reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); path_generator_ = std::make_shared( @@ -514,7 +622,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt transformed_object.kinematics.pose_with_covariance.pose = pose_in_map.pose; } - const auto & label = transformed_object.classification.front().label; + // get tracking label and update it for the prediction + const auto & label_ = transformed_object.classification.front().label; + const auto label = changeLabelForPrediction(label_, object, lanelet_map_ptr_); // For crosswalk user if (label == ObjectClassification::PEDESTRIAN || label == ObjectClassification::BICYCLE) { @@ -1057,6 +1167,20 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( * @return predicted manuever (lane follow, left/right lane change) */ Maneuver MapBasedPredictionNode::predictObjectManeuver( + const TrackedObject & object, const LaneletData & current_lanelet_data, + const double object_detected_time) +{ + if (lane_change_detection_method_ == "time_to_change_lane") { + return predictObjectManeuverByTimeToLaneChange( + object, current_lanelet_data, object_detected_time); + } else if (lane_change_detection_method_ == "lat_diff_distance") { + return predictObjectManeuverByLatDiffDistance( + object, current_lanelet_data, object_detected_time); + } + throw std::logic_error("Lane change detection method is invalid."); +} + +Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { @@ -1127,6 +1251,113 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( return Maneuver::LANE_FOLLOW; } +Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( + const TrackedObject & object, const LaneletData & current_lanelet_data, + const double object_detected_time) +{ + // Step1. Check if we have the object in the buffer + const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + if (objects_history_.count(object_id) == 0) { + return Maneuver::LANE_FOLLOW; + } + + const std::deque & object_info = objects_history_.at(object_id); + const double current_time = (this->get_clock()->now()).seconds(); + + // Step2. Get the previous id + int prev_id = static_cast(object_info.size()) - 1; + while (prev_id >= 0) { + const double prev_time_delay = object_info.at(prev_id).time_delay; + const double prev_time = + rclcpp::Time(object_info.at(prev_id).header.stamp).seconds() + prev_time_delay; + // if (object_detected_time - prev_time > history_time_length_) { + if (current_time - prev_time > history_time_length_) { + break; + } + --prev_id; + } + + if (prev_id < 0) { + return Maneuver::LANE_FOLLOW; + } + + // Step3. Get closest previous lanelet ID + const auto & prev_info = object_info.at(static_cast(prev_id)); + const auto prev_pose = compensateTimeDelay(prev_info.pose, prev_info.twist, prev_info.time_delay); + const lanelet::ConstLanelets prev_lanelets = + object_info.at(static_cast(prev_id)).current_lanelets; + if (prev_lanelets.empty()) { + return Maneuver::LANE_FOLLOW; + } + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + double closest_prev_yaw = std::numeric_limits::max(); + for (const auto & lanelet : prev_lanelets) { + const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position); + const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + if (normalized_delta_yaw < closest_prev_yaw) { + closest_prev_yaw = normalized_delta_yaw; + prev_lanelet = lanelet; + } + } + + // Step4. Check if the vehicle has changed lane + const auto current_lanelet = current_lanelet_data.lanelet; + const double current_time_delay = std::max(current_time - object_detected_time, 0.0); + const auto current_pose = compensateTimeDelay( + object.kinematics.pose_with_covariance.pose, object.kinematics.twist_with_covariance.twist, + current_time_delay); + const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose); + lanelet::routing::LaneletPaths possible_paths = + routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); + bool has_lane_changed = true; + for (const auto & path : possible_paths) { + for (const auto & lanelet : path) { + if (lanelet == current_lanelet) { + has_lane_changed = false; + break; + } + } + } + + if (has_lane_changed) { + return Maneuver::LANE_FOLLOW; + } + + // Step5. Lane Change Detection + const lanelet::ConstLineString2d prev_left_bound = prev_lanelet.leftBound2d(); + const lanelet::ConstLineString2d prev_right_bound = prev_lanelet.rightBound2d(); + const lanelet::ConstLineString2d current_left_bound = current_lanelet.leftBound2d(); + const lanelet::ConstLineString2d current_right_bound = current_lanelet.rightBound2d(); + const double prev_left_dist = calcLeftLateralOffset(prev_left_bound, prev_pose); + const double prev_right_dist = calcRightLateralOffset(prev_right_bound, prev_pose); + const double current_left_dist = calcLeftLateralOffset(current_left_bound, current_pose); + const double current_right_dist = calcRightLateralOffset(current_right_bound, current_pose); + const double prev_lane_width = std::fabs(prev_left_dist) + std::fabs(prev_right_dist); + const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); + if (prev_lane_width < 1e-3 || current_lane_width < 1e-3) { + RCLCPP_ERROR(get_logger(), "[Map Based Prediction]: Lane Width is too small"); + return Maneuver::LANE_FOLLOW; + } + + const double current_left_dist_ratio = current_left_dist / current_lane_width; + const double current_right_dist_ratio = current_right_dist / current_lane_width; + const double diff_left_current_prev = current_left_dist - prev_left_dist; + const double diff_right_current_prev = current_right_dist - prev_right_dist; + + if ( + current_left_dist_ratio > dist_ratio_threshold_to_left_bound_ && + diff_left_current_prev > diff_dist_threshold_to_left_bound_) { + return Maneuver::LEFT_LANE_CHANGE; + } else if ( + current_right_dist_ratio < dist_ratio_threshold_to_right_bound_ && + diff_right_current_prev < diff_dist_threshold_to_right_bound_) { + return Maneuver::RIGHT_LANE_CHANGE; + } + + return Maneuver::LANE_FOLLOW; +} + geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist, const double dt) const diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 27c9d2e836358..b8732b6561878 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -3,16 +3,15 @@ multi_object_tracker 1.0.0 - The ROS2 multi_object_tracker package + The ROS 2 multi_object_tracker package Yukihiro Saito - Jilada Eccleston + Yoshi Ri Apache License 2.0 ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake - autoware_auto_perception_msgs eigen kalman_filter diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index c147035345901..458509cf06e86 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,7 +44,8 @@ BigVehicleTracker::BigVehicleTracker( : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), last_update_time_(time), - z_(object.kinematics.pose_with_covariance.pose.position.z) + z_(object.kinematics.pose_with_covariance.pose.position.z), + tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index f2a8488ec6089..000f728892d11 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,7 +44,8 @@ NormalVehicleTracker::NormalVehicleTracker( : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), last_update_time_(time), - z_(object.kinematics.pose_with_covariance.pose.position.z) + z_(object.kinematics.pose_with_covariance.pose.position.z), + tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml index b0d9cd83e78a0..927f5c69f54e7 100644 --- a/perception/object_merger/package.xml +++ b/perception/object_merger/package.xml @@ -5,13 +5,13 @@ 0.1.0 The object_merger package Yukihiro Saito + Yoshi Ri Apache License 2.0 ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake - autoware_auto_perception_msgs eigen mussp diff --git a/perception/object_range_splitter/package.xml b/perception/object_range_splitter/package.xml index 9a2ac13b35113..d2bb2b401fb2b 100644 --- a/perception/object_range_splitter/package.xml +++ b/perception/object_range_splitter/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs rclcpp diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index 748e9d93c227c..07820d5625fc9 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -3,9 +3,10 @@ occupancy_grid_map_outlier_filter 0.1.0 - The ROS2 occupancy_grid_map_outlier_filter package + The ROS 2 occupancy_grid_map_outlier_filter package amc-nu Yukihiro Saito + Yoshi Ri Apache License 2.0 @@ -14,8 +15,7 @@ William Woodall ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs diagnostic_updater diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 44afe7301d8c7..b9caaac29cb01 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -18,7 +18,7 @@ You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which mean ![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg) -### Each config paramters +### Each config parameters Config parameters are managed in `config/*.yaml` and here shows its outline. diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp index fd12d7529bcc4..31e6fd8cce8fa 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -17,8 +17,8 @@ #include "updater/occupancy_grid_map_updater_interface.hpp" -#include -#include +#include +#include namespace costmap_2d { diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index 127380427bb2f..2bd3a9aa8133c 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -5,13 +5,13 @@ 0.1.0 generate probabilistic occupancy grid map Yukihiro Saito + Yoshi Ri Apache License 2.0 Yukihiro Saito ament_cmake_auto - - autoware_cmake + autoware_cmake eigen3_cmake_module laser_geometry diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index 78db70f8f9c36..b22938cbf0e5a 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -56,10 +56,10 @@ ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.laun ### Input -| Name | Type | Description | -| ----------------------- | ---------------------------------------------------- | ---------------------------------------------------------------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. | -| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/TrackedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` | +| Name | Type | Description | +| ----------------------- | ----------------------------------------------------- | ---------------------------------------------------------------------- | +| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. | +| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` | ### Output diff --git a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp index aab580d34fe09..1a90e9f6bd046 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp @@ -23,7 +23,6 @@ #include "rclcpp/rclcpp.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" -#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp" #include #include @@ -34,8 +33,6 @@ namespace radar_fusion_to_detected_object { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node { @@ -50,22 +47,22 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node private: // Subscriber message_filters::Subscriber sub_object_{}; - message_filters::Subscriber sub_radar_{}; + message_filters::Subscriber sub_radar_{}; using SyncPolicy = - message_filters::sync_policies::ApproximateTime; + message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; typename std::shared_ptr sync_ptr_; // Callback void onData( const DetectedObjects::ConstSharedPtr object_msg, - const TrackedObjects::ConstSharedPtr radar_msg); + const DetectedObjects::ConstSharedPtr radar_msg); bool isDataReady(); // Data Buffer DetectedObjects::ConstSharedPtr detected_objects_{}; - TrackedObjects::ConstSharedPtr radar_objects_{}; + DetectedObjects::ConstSharedPtr radar_objects_{}; // Publisher rclcpp::Publisher::SharedPtr pub_objects_{}; @@ -87,7 +84,7 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node // Lapper RadarFusionToDetectedObject::RadarInput setRadarInput( - const TrackedObject & radar_object, const std_msgs::msg::Header & header_); + const DetectedObject & radar_object, const std_msgs::msg::Header & header_); }; } // namespace radar_fusion_to_detected_object diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index f7c7e10062e7b..21641be7b3b7c 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -10,6 +10,7 @@ Satoshi Tanaka ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs eigen @@ -20,7 +21,6 @@ std_msgs tier4_autoware_utils - autoware_cmake ament_lint_common autoware_lint_common diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index c113527cb5424..8e909e4fc08c8 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -51,7 +51,6 @@ namespace radar_fusion_to_detected_object { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObjects; RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( const rclcpp::NodeOptions & node_options) @@ -181,7 +180,7 @@ bool RadarObjectFusionToDetectedObjectNode::isDataReady() } void RadarObjectFusionToDetectedObjectNode::onData( - const DetectedObjects::ConstSharedPtr object_msg, const TrackedObjects::ConstSharedPtr radar_msg) + const DetectedObjects::ConstSharedPtr object_msg, const DetectedObjects::ConstSharedPtr radar_msg) { detected_objects_ = object_msg; radar_objects_ = radar_msg; @@ -211,7 +210,7 @@ void RadarObjectFusionToDetectedObjectNode::onData( } RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::setRadarInput( - const TrackedObject & radar_object, const std_msgs::msg::Header & header_) + const DetectedObject & radar_object, const std_msgs::msg::Header & header_) { RadarFusionToDetectedObject::RadarInput output{}; output.pose_with_covariance = radar_object.kinematics.pose_with_covariance; diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index 79275858ee0ee..a16cd5945801b 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/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). +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). - Calculation cost is O(n). - n: The number of radar objects @@ -13,7 +13,8 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic - `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic - Output - - `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): The topic converted to Autoware's message + - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message + - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message ### Parameters 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 a45d7c8fc8c73..d5738c80dcca9 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 @@ -18,6 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "autoware_auto_perception_msgs/msg/object_classification.hpp" #include "autoware_auto_perception_msgs/msg/shape.hpp" #include "autoware_auto_perception_msgs/msg/tracked_object.hpp" @@ -33,6 +34,9 @@ namespace radar_tracks_msgs_converter { +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; +using autoware_auto_perception_msgs::msg::DetectedObjects; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_perception_msgs::msg::TrackedObject; @@ -69,7 +73,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_; // Publisher - rclcpp::Publisher::SharedPtr pub_radar_{}; + rclcpp::Publisher::SharedPtr pub_tracked_objects_{}; + rclcpp::Publisher::SharedPtr pub_detected_objects_{}; // Timer rclcpp::TimerBase::SharedPtr timer_{}; @@ -86,7 +91,9 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node NodeParam node_param_{}; // Core + geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance(); TrackedObjects convertRadarTrackToTrackedObjects(); + DetectedObjects convertRadarTrackToDetectedObjects(); 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 ba4cc4762ea7d..c9f4a31354247 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 @@ -1,14 +1,16 @@ - + + - + + diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index 84ee931b7950f..4092125cde33f 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -10,6 +10,7 @@ Satoshi Tanaka ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs geometry_msgs @@ -22,7 +23,6 @@ tf2_ros tier4_autoware_utils - autoware_cmake ament_lint_auto autoware_lint_common 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 cfe6ca3e4cea0..eb4a497e70495 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 @@ -88,7 +88,8 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt transform_listener_ = std::make_shared(this); // Publisher - pub_radar_ = create_publisher("~/output/radar_objects", 1); + pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); + pub_detected_objects_ = create_publisher("~/output/radar_detected_objects", 1); // Timer const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period(); @@ -152,11 +153,99 @@ void RadarTracksMsgsConverterNode::onTimer() node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects(); + DetectedObjects detected_objects = convertRadarTrackToDetectedObjects(); if (!tracked_objects.objects.empty()) { - pub_radar_->publish(tracked_objects); + pub_tracked_objects_->publish(tracked_objects); + pub_detected_objects_->publish(detected_objects); } } +DetectedObjects RadarTracksMsgsConverterNode::convertRadarTrackToDetectedObjects() +{ + DetectedObjects detected_objects; + detected_objects.header = radar_data_->header; + detected_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) { + DetectedObject detected_object; + + detected_object.existence_probability = 1.0; + + detected_object.shape.type = Shape::BOUNDING_BOX; + detected_object.shape.dimensions = radar_track.size; + + // kinematics + DetectedObjectKinematics kinematics; + kinematics.has_twist = true; + kinematics.has_twist_covariance = true; + + // convert by tf + geometry_msgs::msg::PoseStamped radar_pose_stamped{}; + radar_pose_stamped.pose.position = radar_track.position; + geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; + tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); + kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; + + { + 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]; + } + + // convert by tf + geometry_msgs::msg::Vector3Stamped radar_velocity_stamped{}; + radar_velocity_stamped.vector = radar_track.velocity; + geometry_msgs::msg::Vector3Stamped transformed_vector3_stamped{}; + tf2::doTransform(radar_velocity_stamped, transformed_vector3_stamped, *transform_); + kinematics.twist_with_covariance.twist.linear = transformed_vector3_stamped.vector; + + // twist compensation + if (node_param_.use_twist_compensation) { + if (odometry_data_) { + kinematics.twist_with_covariance.twist.linear.x += odometry_data_->twist.twist.linear.x; + kinematics.twist_with_covariance.twist.linear.y += odometry_data_->twist.twist.linear.y; + kinematics.twist_with_covariance.twist.linear.z += odometry_data_->twist.twist.linear.z; + } else { + RCLCPP_INFO(get_logger(), "Odometry data is not coming"); + } + } + + { + 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]; + } + detected_object.kinematics = kinematics; + + // classification + ObjectClassification classification; + classification.probability = 1.0; + classification.label = convertClassification(radar_track.classification); + detected_object.classification.emplace_back(classification); + + detected_objects.objects.emplace_back(detected_object); + } + return detected_objects; +} + TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() { TrackedObjects tracked_objects; diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index aa51c462b07dc..334b6cc2af123 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -3,13 +3,13 @@ shape_estimation 0.1.0 - This package implements a shape estimation algorithm as a ROS2 node + This package implements a shape estimation algorithm as a ROS 2 node Yukihiro Saito + Yoshi Ri Apache License 2.0 ament_cmake - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs builtin_interfaces diff --git a/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp b/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp index 4702f6f0f29de..aea3142b1f35e 100644 --- a/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp +++ b/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp @@ -27,7 +27,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp index 4ff96bf15257b..b486595a2cb7b 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp @@ -39,10 +39,10 @@ #include #include -#include #include #include +#include using nvinfer1::DataType; using nvinfer1::DimsExprs; @@ -62,14 +62,14 @@ const char * NMS_PLUGIN_NAMESPACE{""}; template void write(char *& buffer, const T & val) { - *reinterpret_cast(buffer) = val; + std::memcpy(buffer, &val, sizeof(T)); buffer += sizeof(T); } template void read(const char *& buffer, T & val) { - val = *reinterpret_cast(buffer); + std::memcpy(&val, buffer, sizeof(T)); buffer += sizeof(T); } diff --git a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp index 78edc568bb7c4..dc12921dc1e96 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp @@ -63,10 +63,10 @@ #include #include -#include #include #include +#include #include using nvinfer1::DataType; @@ -87,14 +87,14 @@ const char * YOLO_LAYER_PLUGIN_NAMESPACE{""}; template void write(char *& buffer, const T & val) { - *reinterpret_cast(buffer) = val; + std::memcpy(buffer, &val, sizeof(T)); buffer += sizeof(T); } template void read(const char *& buffer, T & val) { - val = *reinterpret_cast(buffer); + std::memcpy(&val, buffer, sizeof(T)); buffer += sizeof(T); } } // namespace diff --git a/perception/tensorrt_yolo/package.xml b/perception/tensorrt_yolo/package.xml index 68ca942435e06..8a6756449b70f 100755 --- a/perception/tensorrt_yolo/package.xml +++ b/perception/tensorrt_yolo/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs cv_bridge diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index c98595e931153..08263d05fbe83 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.17) project(tensorrt_yolox) find_package(tensorrt_common) @@ -11,6 +11,13 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(OpenCV REQUIRED) +include(CheckLanguage) +check_language(CUDA) +if(CMAKE_CUDA_COMPILER) + enable_language(CUDA) +else() + message(WARNING "CUDA is not found. preprocess acceleration using CUDA will not be available.") +endif() find_package(OpenMP) if(OpenMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") @@ -68,9 +75,31 @@ ament_target_dependencies(${PROJECT_NAME} OpenCV ) -target_link_libraries(${PROJECT_NAME} - ${tensorrt_common_LIBRARIES} -) +if(CMAKE_CUDA_COMPILER) + # Officially, add_library supports .cu file compilation. + # However, as of cmake 3.22.1, it seems to fail compilation because compiler flags for + # C++ are directly passed to nvcc (they are originally space separated + # but nvcc assume comma separated as argument of `-Xcompiler` option). + # That is why `cuda_add_library` is used here. + cuda_add_library(${PROJECT_NAME}_gpu_preprocess + SHARED + src/preprocess.cu + ) + + target_include_directories(${PROJECT_NAME}_gpu_preprocess PUBLIC + $ + $ + ) + + target_link_libraries(${PROJECT_NAME} + ${tensorrt_common_LIBRARIES} + ${PROJECT_NAME}_gpu_preprocess + ) +else() + target_link_libraries(${PROJECT_NAME} + ${tensorrt_common_LIBRARIES} + ) +endif() target_compile_definitions(${PROJECT_NAME} PRIVATE TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR} diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index 03c04b7ed7d09..a8332ad9eeec1 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -40,12 +40,20 @@ Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Se ### Node Parameters -| Name | Type | Default Value | Description | -| --------------- | ------ | ------------- | ------------------------------------------------------------------ | -| `model_path` | string | "" | The onnx file name for yolox model | -| `label_path` | string | "" | The label file with label names for detected objects written on it | -| `trt_precision` | string | "fp32" | The inference mode: "fp32", "fp16", "int8" | -| `build_only` | bool | false | shutdown node after TensorRT engine file is built | +| Name | Type | Default Value | Description | +| ----------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `model_path` | string | "" | The onnx file name for yolox model | +| `label_path` | string | "" | The label file with label names for detected objects written on it | +| `precision` | string | "fp16" | The inference mode: "fp32", "fp16", "int8" | +| `build_only` | bool | false | shutdown node after TensorRT engine file is built | +| `calibration_algorithm` | string | "MinMax" | Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: Entropy",("Legacy" \| "Percentile"), "MinMax"] | +| `dla_core_id` | int | -1 | If positive ID value is specified, the node assign inference task to the DLA core | +| `quantize_first_layer` | bool | false | If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8 | +| `quantize_last_layer` | bool | false | If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8 | +| `profile_per_layer` | bool | false | If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. | +| `clip_value` | double | 0.0 | If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration | +| `preprocess_on_gpu` | bool | true | If true, pre-processing is performed on GPU | +| `calibration_image_list_path` | string | "" | Path to a file which contains path to images. Those images will be used for int8 quantization. | ## Assumptions / Known limits diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp new file mode 100644 index 0000000000000..d859cb5c9c9c5 --- /dev/null +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp @@ -0,0 +1,493 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +#ifndef TENSORRT_YOLOX__CALIBRATOR_HPP_ +#define TENSORRT_YOLOX__CALIBRATOR_HPP_ + +#include "cuda_utils/cuda_check_error.hpp" +#include "cuda_utils/cuda_unique_ptr.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace tensorrt_yolox +{ +class ImageStream +{ +public: + ImageStream( + int batch_size, nvinfer1::Dims input_dims, const std::vector calibration_images) + : batch_size_(batch_size), + calibration_images_(calibration_images), + current_batch_(0), + max_batches_(calibration_images.size() / batch_size_), + input_dims_(input_dims) + { + batch_.resize(batch_size_ * input_dims_.d[1] * input_dims_.d[2] * input_dims_.d[3]); + } + + int getBatchSize() const { return batch_size_; } + + int getMaxBatches() const { return max_batches_; } + + float * getBatch() { return &batch_[0]; } + + nvinfer1::Dims getInputDims() { return input_dims_; } + + /** + * @brief Preprocess in calibration + * @param[in] images input images + * @param[in] input_dims input dimensions + * @param[in] norm normalization (0.0-1.0) + * @return vector preprocessed data + */ + std::vector preprocess( + const std::vector & images, nvinfer1::Dims input_dims, double norm) + { + std::vector input_h_; + const auto batch_size = images.size(); + input_dims.d[0] = batch_size; + const float input_height = static_cast(input_dims.d[2]); + const float input_width = static_cast(input_dims.d[3]); + std::vector dst_images; + std::vector scales_; + scales_.clear(); + for (const auto & image : images) { + cv::Mat dst_image; + const float scale = std::min(input_width / image.cols, input_height / image.rows); + scales_.emplace_back(scale); + const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); + cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + const auto bottom = input_height - dst_image.rows; + const auto right = input_width - dst_image.cols; + copyMakeBorder( + dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); + dst_images.emplace_back(dst_image); + } + const auto chw_images = + cv::dnn::blobFromImages(dst_images, norm, cv::Size(), cv::Scalar(), false, false, CV_32F); + + const auto data_length = chw_images.total(); + input_h_.reserve(data_length); + const auto flat = chw_images.reshape(1, data_length); + input_h_ = chw_images.isContinuous() ? flat : flat.clone(); + return input_h_; + } + + /** + * @brief Decode data in calibration + * @param[in] scale normalization (0.0-1.0) + * @return bool succ or fail + */ + bool next(double scale) + { + if (current_batch_ == max_batches_) { + return false; + } + + for (int i = 0; i < batch_size_; ++i) { + auto image = + cv::imread(calibration_images_[batch_size_ * current_batch_ + i].c_str(), cv::IMREAD_COLOR); + std::cout << current_batch_ << " " << i << " Preprocess " + << calibration_images_[batch_size_ * current_batch_ + i].c_str() << std::endl; + auto input = preprocess({image}, input_dims_, scale); + batch_.insert( + batch_.begin() + i * input_dims_.d[1] * input_dims_.d[2] * input_dims_.d[3], input.begin(), + input.end()); + } + + ++current_batch_; + return true; + } + + /** + * @brief Reset calibration + */ + void reset() { current_batch_ = 0; } + +private: + int batch_size_; + std::vector calibration_images_; + int current_batch_; + int max_batches_; + nvinfer1::Dims input_dims_; + std::vector batch_; +}; + +/**Percentile calibration using legacy calibrator*/ +/** + * @class Int8LegacyCalibrator + * @brief Calibrator for Percentle + * @warning We are confirming bug on Tegra like Xavier and Orin. We recommand use MinMax calibrator + */ +class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator +{ +public: + Int8LegacyCalibrator( + ImageStream & stream, const std::string calibration_cache_file, + const std::string histogram_cache_file, double scale = 1.0, bool read_cache = true, + double quantile = 0.999999, double cutoff = 0.999999) + : stream_(stream), + calibration_cache_file_(calibration_cache_file), + histogranm_cache_file_(histogram_cache_file), + read_cache_(read_cache) + { + auto d = stream_.getInputDims(); + input_count_ = stream_.getBatchSize() * d.d[1] * d.d[2] * d.d[3]; + CHECK_CUDA_ERROR(cudaMalloc(&device_input_, input_count_ * sizeof(float))); + scale_ = scale; + quantile_ = quantile; + cutoff_ = cutoff; + auto algType = getAlgorithm(); + switch (algType) { + case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): + std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): + std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): + std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): + std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + break; + default: + std::cout << "No CalibrationAlgType" << std::endl; + break; + } + } + int getBatchSize() const noexcept override { return stream_.getBatchSize(); } + + virtual ~Int8LegacyCalibrator() { CHECK_CUDA_ERROR(cudaFree(device_input_)); } + + bool getBatch(void * bindings[], const char * names[], int nb_bindings) noexcept override + { + (void)names; + (void)nb_bindings; + + if (!stream_.next(scale_)) { + return false; + } + try { + CHECK_CUDA_ERROR(cudaMemcpy( + device_input_, stream_.getBatch(), input_count_ * sizeof(float), cudaMemcpyHostToDevice)); + } catch (const std::exception & e) { + // Do nothing + } + bindings[0] = device_input_; + return true; + } + + const void * readCalibrationCache(size_t & length) noexcept override + { + calib_cache_.clear(); + std::ifstream input(calibration_cache_file_, std::ios::binary); + input >> std::noskipws; + if (read_cache_ && input.good()) { + std::copy( + std::istream_iterator(input), std::istream_iterator(), + std::back_inserter(calib_cache_)); + } + + length = calib_cache_.size(); + if (length) { + std::cout << "Using cached calibration table to build the engine" << std::endl; + } else { + std::cout << "New calibration table will be created to build the engine" << std::endl; + } + return length ? &calib_cache_[0] : nullptr; + } + + void writeCalibrationCache(const void * cache, size_t length) noexcept override + { + std::ofstream output(calibration_cache_file_, std::ios::binary); + output.write(reinterpret_cast(cache), length); + } + + double getQuantile() const noexcept + { + printf("Quantile %f\n", quantile_); + return quantile_; + } + + double getRegressionCutoff(void) const noexcept + { + printf("Cutoff %f\n", cutoff_); + return cutoff_; + } + + const void * readHistogramCache(std::size_t & length) noexcept + { + hist_cache_.clear(); + std::ifstream input(histogranm_cache_file_, std::ios::binary); + input >> std::noskipws; + if (read_cache_ && input.good()) { + std::copy( + std::istream_iterator(input), std::istream_iterator(), + std::back_inserter(hist_cache_)); + } + + length = hist_cache_.size(); + if (length) { + std::cout << "Using cached histogram table to build the engine" << std::endl; + } else { + std::cout << "New histogram table will be created to build the engine" << std::endl; + } + return length ? &hist_cache_[0] : nullptr; + } + void writeHistogramCache(void const * ptr, std::size_t length) noexcept + { + std::ofstream output(histogranm_cache_file_, std::ios::binary); + output.write(reinterpret_cast(ptr), length); + } + +private: + ImageStream stream_; + const std::string calibration_cache_file_; + const std::string histogranm_cache_file_; + bool read_cache_{true}; + size_t input_count_; + void * device_input_{nullptr}; + std::vector calib_cache_; + std::vector hist_cache_; + double scale_; + double quantile_; + double cutoff_; +}; + +/** + * @class Int8LegacyCalibrator + * @brief Calibrator for Percentle + * @warning This calibrator causes crucial accuracy drop for YOLOX. + */ +class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 +{ +public: + Int8EntropyCalibrator( + ImageStream & stream, const std::string calibration_cache_file, double scale = 1.0, + bool read_cache = true) + : stream_(stream), calibration_cache_file_(calibration_cache_file), read_cache_(read_cache) + { + auto d = stream_.getInputDims(); + input_count_ = stream_.getBatchSize() * d.d[1] * d.d[2] * d.d[3]; + CHECK_CUDA_ERROR(cudaMalloc(&device_input_, input_count_ * sizeof(float))); + scale_ = scale; + auto algType = getAlgorithm(); + switch (algType) { + case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): + std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): + std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): + std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): + std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + break; + default: + std::cout << "No CalibrationAlgType" << std::endl; + break; + } + } + int getBatchSize() const noexcept override { return stream_.getBatchSize(); } + + virtual ~Int8EntropyCalibrator() { CHECK_CUDA_ERROR(cudaFree(device_input_)); } + + bool getBatch(void * bindings[], const char * names[], int nb_bindings) noexcept override + { + (void)names; + (void)nb_bindings; + + if (!stream_.next(scale_)) { + return false; + } + try { + CHECK_CUDA_ERROR(cudaMemcpy( + device_input_, stream_.getBatch(), input_count_ * sizeof(float), cudaMemcpyHostToDevice)); + } catch (const std::exception & e) { + // Do nothing + } + bindings[0] = device_input_; + return true; + } + + const void * readCalibrationCache(size_t & length) noexcept override + { + calib_cache_.clear(); + std::ifstream input(calibration_cache_file_, std::ios::binary); + input >> std::noskipws; + if (read_cache_ && input.good()) { + std::copy( + std::istream_iterator(input), std::istream_iterator(), + std::back_inserter(calib_cache_)); + } + + length = calib_cache_.size(); + if (length) { + std::cout << "Using cached calibration table to build the engine" << std::endl; + } else { + std::cout << "New calibration table will be created to build the engine" << std::endl; + } + return length ? &calib_cache_[0] : nullptr; + } + + void writeCalibrationCache(const void * cache, size_t length) noexcept override + { + std::ofstream output(calibration_cache_file_, std::ios::binary); + output.write(reinterpret_cast(cache), length); + } + +private: + ImageStream stream_; + const std::string calibration_cache_file_; + bool read_cache_{true}; + size_t input_count_; + void * device_input_{nullptr}; + std::vector calib_cache_; + std::vector hist_cache_; + double scale_; +}; + +/** + * @class Int8MinMaxCalibrator + * @brief Calibrator for MinMax + * @warning We strongly recommand MinMax calibrator for YOLOX + */ +class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator +{ +public: + Int8MinMaxCalibrator( + ImageStream & stream, const std::string calibration_cache_file, double scale = 1.0, + bool read_cache = true) + : stream_(stream), calibration_cache_file_(calibration_cache_file), read_cache_(read_cache) + { + auto d = stream_.getInputDims(); + input_count_ = stream_.getBatchSize() * d.d[1] * d.d[2] * d.d[3]; + CHECK_CUDA_ERROR(cudaMalloc(&device_input_, input_count_ * sizeof(float))); + scale_ = scale; + auto algType = getAlgorithm(); + switch (algType) { + case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): + std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): + std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): + std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + break; + case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): + std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + break; + default: + std::cout << "No CalibrationAlgType" << std::endl; + break; + } + } + int getBatchSize() const noexcept override { return stream_.getBatchSize(); } + + virtual ~Int8MinMaxCalibrator() { CHECK_CUDA_ERROR(cudaFree(device_input_)); } + + bool getBatch(void * bindings[], const char * names[], int nb_bindings) noexcept override + { + (void)names; + (void)nb_bindings; + + if (!stream_.next(scale_)) { + return false; + } + try { + CHECK_CUDA_ERROR(cudaMemcpy( + device_input_, stream_.getBatch(), input_count_ * sizeof(float), cudaMemcpyHostToDevice)); + } catch (const std::exception & e) { + // Do nothing + } + bindings[0] = device_input_; + return true; + } + + const void * readCalibrationCache(size_t & length) noexcept override + { + calib_cache_.clear(); + std::ifstream input(calibration_cache_file_, std::ios::binary); + input >> std::noskipws; + if (read_cache_ && input.good()) { + std::copy( + std::istream_iterator(input), std::istream_iterator(), + std::back_inserter(calib_cache_)); + } + + length = calib_cache_.size(); + if (length) { + std::cout << "Using cached calibration table to build the engine" << std::endl; + } else { + std::cout << "New calibration table will be created to build the engine" << std::endl; + } + return length ? &calib_cache_[0] : nullptr; + } + + void writeCalibrationCache(const void * cache, size_t length) noexcept override + { + std::ofstream output(calibration_cache_file_, std::ios::binary); + output.write(reinterpret_cast(cache), length); + } + +private: + ImageStream stream_; + const std::string calibration_cache_file_; + bool read_cache_{true}; + size_t input_count_; + void * device_input_{nullptr}; + std::vector calib_cache_; + std::vector hist_cache_; + double scale_; +}; +} // namespace tensorrt_yolox + +#endif // TENSORRT_YOLOX__CALIBRATOR_HPP_ diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp new file mode 100644 index 0000000000000..f068268d2fc65 --- /dev/null +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp @@ -0,0 +1,117 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TENSORRT_YOLOX__PREPROCESS_HPP_ +#define TENSORRT_YOLOX__PREPROCESS_HPP_ + +#include +#include +#include +#include +#include + +namespace tensorrt_yolox +{ +/** + * @brief Resize a image using bilinear interpolation on gpus + * @param[out] dst Resized image + * @param[in] src image + * @param[in] d_w width for resized image + * @param[in] d_h height for resized image + * @param[in] d_c channel for resized image + * @param[in] s_w width for input image + * @param[in] s_h height for input image + * @param[in] s_c channel for input image + * @param[in] stream cuda stream + */ +extern void resize_bilinear_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + cudaStream_t stream); + +/** + * @brief Letterbox a image on gpus + * @param[out] dst letterboxed image + * @param[in] src image + * @param[in] d_w width for letterboxing + * @param[in] d_h height foletterboxing + * @param[in] d_c channel foletterboxing + * @param[in] s_w width for input image + * @param[in] s_h height for input image + * @param[in] s_c channel for input image + * @param[in] stream cuda stream + */ +extern void letterbox_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + cudaStream_t stream); + +/** + * @brief NHWC to NHWC conversion + * @param[out] dst converted image + * @param[in] src image + * @param[in] d_w width for a image + * @param[in] d_h height for a image + * @param[in] d_c channel for a image + * @param[in] stream cuda stream + */ +extern void nchw_to_nhwc_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, cudaStream_t stream); + +/** + * @brief Unsigned char to float32 for inference + * @param[out] dst32 converted image + * @param[in] src image + * @param[in] d_w width for a image + * @param[in] d_h height for a image + * @param[in] d_c channel for a image + * @param[in] stream cuda stream + */ +extern void to_float_gpu( + float * dst32, unsigned char * src, int d_w, int d_h, int d_c, cudaStream_t stream); + +/** + * @brief Resize and letterbox a image using bilinear interpolation on gpus + * @param[out] dst processsed image + * @param[in] src image + * @param[in] d_w width for output + * @param[in] d_h height for output + * @param[in] d_c channel for output + * @param[in] s_w width for input + * @param[in] s_h height for input + * @param[in] s_c channel for input + * @param[in] stream cuda stream + */ +extern void resize_bilinear_letterbox_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + cudaStream_t stream); + +/** + * @brief Optimized preprocessing including resize, letterbox, nhwc2nchw, toFloat and normalization + * for YOLOX on gpus + * @param[out] dst processsed image + * @param[in] src image + * @param[in] d_w width for output + * @param[in] d_h height for output + * @param[in] d_c channel for output + * @param[in] s_w width for input + * @param[in] s_h height for input + * @param[in] s_c channel for input + * @param[in] norm normalization + * @param[in] stream cuda stream + */ +extern void resize_bilinear_letterbox_nhwc_to_nchw32_gpu( + float * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + float norm, cudaStream_t stream); +} // namespace tensorrt_yolox + +#endif // TENSORRT_YOLOX__PREPROCESS_HPP_ diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp index 8ee0c8e1ed59e..2bdb9a83bfbf9 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -51,25 +51,74 @@ struct GridAndStride int stride; }; +/** + * @class TrtYoloX + * @brief TensorRT YOLOX for faster inference + * @warning Regarding quantization, we recommend use MinMax calibration due to accuracy drop with + * Entropy calibration. + */ class TrtYoloX { public: + /** + * @brief Construct TrtYoloX. + * @param[in] mode_path ONNX model_path + * @param[in] precision precision for inference + * @param[in] num_class classifier-ed num + * @param[in] score_threshold threshold for detection + * @param[in] nms_threshold threshold for NMS + * @param[in] build_config configuration including precision, calibration method, DLA, remaining + * fp16 for first layer, remaining fp16 for last layer and profiler for builder + * @param[in] use_gpu_preprocess whether use cuda gpu for preprocessing + * @param[in] calibration_image_list_file path for calibration files (only require for + * quantization) + * @param[in] norm_factor scaling factor for preprocess + * @param[in] cache_dir unused variable + * @param[in] batch_config configuration for batched execution + * @param[in] max_workspace_size maximum workspace for building TensorRT engine + */ TrtYoloX( const std::string & model_path, const std::string & precision, const int num_class = 8, const float score_threshold = 0.3, const float nms_threshold = 0.7, + const tensorrt_common::BuildConfig build_config = tensorrt_common::BuildConfig(), + const bool use_gpu_preprocess = false, + const std::string & calibration_image_list_file = std::string(), const double norm_factor = 1.0, const std::string & cache_dir = "", const tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, const size_t max_workspace_size = (1 << 30)); + /** + * @brief run inference including pre-process and post-process + * @param[out] objects results for object detection + * @param[in] images batched images + */ bool doInference(const std::vector & images, ObjectArrays & objects); + /** + * @brief allocate buffer for preprocess on GPU + * @param[in] width original image width + * @param[in] height original image height + * @warning if we don't allocate buffers using it, "preprocess_gpu" allocates buffers at the + * beginning + */ + void initPreprocesBuffer(int width, int height); + + /** + * @brief output TensorRT profiles for each layer + */ + void printProfiling(void); + private: void preprocess(const std::vector & images); + + // NOTE: Currently only supports a single batch image + void preprocessGpu(const std::vector & images); + bool feedforward(const std::vector & images, ObjectArrays & objects); bool feedforwardAndDecode(const std::vector & images, ObjectArrays & objects); void decodeOutputs(float * prob, ObjectArray & objects, float scale, cv::Size & img_size) const; void generateGridsAndStride( - const int target_w, const int target_h, std::vector & strides, + const int target_w, const int target_h, const std::vector & strides, std::vector & grid_strides) const; void generateYoloxProposals( std::vector grid_strides, float * feat_blob, float prob_threshold, @@ -82,6 +131,7 @@ class TrtYoloX } qsortDescentInplace(objects, 0, objects.size() - 1); } + inline float intersectionArea(const Object & a, const Object & b) const { cv::Rect a_rect(a.x_offset, a.y_offset, a.width, a.height); @@ -89,6 +139,7 @@ class TrtYoloX cv::Rect_ inter = a_rect & b_rect; return inter.area(); } + // cspell: ignore Bboxes void nmsSortedBboxes( const ObjectArray & face_objects, std::vector & picked, float nms_threshold) const; @@ -117,6 +168,20 @@ class TrtYoloX float nms_threshold_; CudaUniquePtrHost out_prob_h_; + + // flag whether prepreceses are performed on GPU + bool use_gpu_preprocess_; + // host buffer for preprecessing on GPU + CudaUniquePtrHost image_buf_h_; + // device buffer for preprecessing on GPU + CudaUniquePtr image_buf_d_; + // normalization factor used for preprocessing + double norm_factor_; + + std::vector output_strides_; + + int src_width_; + int src_height_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index 23aaea5504252..9b440f1dbdfec 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -24,7 +24,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/perception/tensorrt_yolox/launch/yolox.launch.xml b/perception/tensorrt_yolox/launch/yolox.launch.xml index b697b1f50eb3d..d8c67e39e0b8a 100644 --- a/perception/tensorrt_yolox/launch/yolox.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox.launch.xml @@ -6,6 +6,27 @@ + + + + + + + + + @@ -21,7 +42,15 @@ - + + + + + + + + + diff --git a/perception/tensorrt_yolox/package.xml b/perception/tensorrt_yolox/package.xml index 09b36fa768bd0..1a67c63f5b970 100644 --- a/perception/tensorrt_yolox/package.xml +++ b/perception/tensorrt_yolox/package.xml @@ -6,19 +6,19 @@ Daisuke Nishimatsu Daisuke Nishimatsu + Dan Umeda Manato Hirabayashi Apache License 2.0 ament_cmake_auto + autoware_cmake cudnn_cmake_module tensorrt_cmake_module cudnn_cmake_module tensorrt_cmake_module - autoware_cmake - autoware_auto_perception_msgs cuda_utils cv_bridge diff --git a/perception/tensorrt_yolox/src/preprocess.cu b/perception/tensorrt_yolox/src/preprocess.cu new file mode 100644 index 0000000000000..7c9d9eff317df --- /dev/null +++ b/perception/tensorrt_yolox/src/preprocess.cu @@ -0,0 +1,371 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include + +namespace tensorrt_yolox +{ +constexpr size_t block = 512; + +dim3 cuda_gridsize(size_t n) +{ + size_t k = (n - 1) / block + 1; + size_t x = k; + size_t y = 1; + if (x > 65535) { + x = ceil(sqrt(k)); + y = (n - 1) / (x * block) + 1; + } + dim3 d; + d.x = x; + d.y = y; + d.z = 1; + return d; +} + +__device__ double lerp1d(int a, int b, float w) +{ + return fma(w, (float)b, fma(-w, (float)a, (float)a)); +} + +__device__ float lerp2d(int f00, int f01, int f10, int f11, float centroid_h, float centroid_w) +{ + centroid_w = (1 + lroundf(centroid_w) - centroid_w) / 2; + centroid_h = (1 + lroundf(centroid_h) - centroid_h) / 2; + + float r0, r1, r; + r0 = lerp1d(f00, f01, centroid_w); + r1 = lerp1d(f10, f11, centroid_w); + + r = lerp1d(r0, r1, centroid_h); //+ 0.00001 + return r; +} + +__global__ void resize_bilinear_kernel( + int N, unsigned char * dst_img, unsigned char * src_img, int dst_h, int dst_w, int src_h, + int src_w, float stride_h, float stride_w) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int C = 3; + int W = dst_w; + + int c = 0; + int n = 0; + + int w = index % W; + int h = index / W; + + float centroid_h, centroid_w; + centroid_h = stride_h * (float)(h + 0.5); + centroid_w = stride_w * (float)(w + 0.5); + + int f00, f01, f10, f11; + + int src_h_idx = lroundf(centroid_h) - 1; + int src_w_idx = lroundf(centroid_w) - 1; + if (src_h_idx < 0) { + src_h_idx = 0; + } + if (src_w_idx < 0) { + src_w_idx = 0; + } + + index = C * w + C * W * h; + // Unroll + for (c = 0; c < C; c++) { + f00 = n * src_h * src_w * C + src_h_idx * src_w * C + src_w_idx * C + c; + f01 = n * src_h * src_w * C + src_h_idx * src_w * C + (src_w_idx + 1) * C + c; + f10 = n * src_h * src_w * C + (src_h_idx + 1) * src_w * C + src_w_idx * C + c; + f11 = n * src_h * src_w * C + (src_h_idx + 1) * src_w * C + (src_w_idx + 1) * C + c; + + float rs = lroundf(lerp2d( + (int)src_img[f00], (int)src_img[f01], (int)src_img[f10], (int)src_img[f11], centroid_h, + centroid_w)); + dst_img[index + c] = (unsigned char)rs; + } +} + +void resize_bilinear_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + cudaStream_t stream) +{ + int N = d_w * d_h; + float stride_h = (float)s_h / (float)d_h; + float stride_w = (float)s_w / (float)d_w; + + resize_bilinear_kernel<<>>( + N, dst, src, d_h, d_w, s_h, s_w, stride_h, stride_w); +} + +__global__ void letterbox_kernel( + int N, unsigned char * dst_img, unsigned char * src_img, int dst_h, int dst_w, int src_h, + int src_w, float scale, int letter_bot, int letter_right) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int C = 3; + int W = dst_w; + int c = 0; + + int w = index % W; + int h = index / W; + + index = (C * w) + (C * W * h); + // Unroll + int index2 = (C * w) + (C * src_w * h); + for (c = 0; c < C; c++) { + dst_img[index + c] = + (w >= letter_right || h >= letter_bot) ? (unsigned int)114 : src_img[index2 + c]; + } +} + +void letterbox_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + cudaStream_t stream) +{ + int N = d_w * d_h; + const float scale = std::min(d_w / (float)s_w, d_h / (float)s_h); + int r_h = (int)(scale * s_h); + int r_w = (int)(scale * s_w); + + float stride_h = (float)s_h / (float)r_h; + float stride_w = (float)s_w / (float)r_w; + letterbox_kernel<<>>( + N, dst, src, d_h, d_w, r_h, r_w, 1.0 / scale, r_h, r_w); +} + +__global__ void nhwc_to_nchw_kernel( + int N, unsigned char * dst_img, unsigned char * src_img, int height, int width) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int chan = 3; + int c = 0; + int x = index % width; + int y = index / width; + int src_index = 0; + int dst_index = 0; + for (c = 0; c < chan; c++) { + src_index = c + (chan * x) + (chan * width * y); + dst_index = x + (width * y) + (width * height * c); + dst_img[dst_index] = src_img[src_index]; + } +} + +void nhwc_to_nchw_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, cudaStream_t stream) +{ + int N = d_w * d_h; + nhwc_to_nchw_kernel<<>>(N, dst, src, d_h, d_w); +} + +__global__ void nchw_to_nhwc_kernel( + int N, unsigned char * dst, unsigned char * src, int height, int width) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int chan = 3; + int c = 0; + int x = index % width; + int y = index / width; + int src_index = 0; + int dst_index = 0; + for (c = 0; c < chan; c++) { + // NHWC + dst_index = c + (chan * x) + (chan * width * y); + // NCHW + src_index = x + (width * y) + (width * height * c); + dst[dst_index] = src[src_index]; + } +} + +void nchw_to_nhwc_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, cudaStream_t stream) +{ + int N = d_w * d_h; + nchw_to_nhwc_kernel<<>>(N, dst, src, d_h, d_w); +} + +__global__ void to_float_kernel(int N, float * dst32, unsigned char * src8, int height, int width) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int chan = 3; + int c = 0; + int x = index % width; + int y = index / width; + int dst_index = 0; + for (c = 0; c < chan; c++) { + // NCHW + dst_index = x + (width * y) + (width * height * c); + dst32[dst_index] = (float)(src8[dst_index]); + } +} + +void to_float_gpu( + float * dst32, unsigned char * src, int d_w, int d_h, int d_c, cudaStream_t stream) +{ + int N = d_w * d_h; + to_float_kernel<<>>(N, dst32, src, d_h, d_w); +} + +__global__ void resize_bilinear_letterbox_kernel( + int N, unsigned char * dst_img, unsigned char * src_img, int dst_h, int dst_w, int src_h, + int src_w, float scale, int letter_bot, int letter_right) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int C = 3; // # ChannelDim + int W = dst_w; + int c = 0; + int n = 0; // index / (C*W*H); + + int w = index % W; + int h = index / W; + + float centroid_h, centroid_w; + centroid_h = scale * (float)(h + 0.5); + centroid_w = scale * (float)(w + 0.5); + + int f00, f01, f10, f11; + + int src_h_idx = (int)lroundf(centroid_h) - 1; + int src_w_idx = (int)lroundf(centroid_w) - 1; + if (src_h_idx < 0) { + src_h_idx = 0; + } + if (src_w_idx < 0) { + src_w_idx = 0; + } + if (src_h_idx >= src_h) { + src_h_idx = src_h - 1; + } + if (src_w_idx >= src_w) { + src_w_idx = src_w - 1; + } + + index = (C * w) + (C * W * h); + // Unroll + for (c = 0; c < C; c++) { + f00 = n * src_h * src_w * C + src_h_idx * src_w * C + src_w_idx * C + c; + f01 = n * src_h * src_w * C + src_h_idx * src_w * C + (src_w_idx + 1) * C + c; + f10 = n * src_h * src_w * C + (src_h_idx + 1) * src_w * C + src_w_idx * C + c; + f11 = n * src_h * src_w * C + (src_h_idx + 1) * src_w * C + (src_w_idx + 1) * C + c; + + float rs = lroundf(lerp2d( + (int)src_img[f00], (int)src_img[f01], (int)src_img[f10], (int)src_img[f11], centroid_h, + centroid_w)); + dst_img[index + c] = (unsigned char)rs; + dst_img[index + c] = (h >= letter_bot) ? (unsigned int)114 : dst_img[index + c]; + dst_img[index + c] = (w >= letter_right) ? (unsigned int)114 : dst_img[index + c]; + } +} + +void resize_bilinear_letterbox_gpu( + unsigned char * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + cudaStream_t stream) +{ + int N = d_w * d_h; + const float scale = std::min(d_w / (float)s_w, d_h / (float)s_h); + int r_h = (int)(scale * s_h); + int r_w = (int)(scale * s_w); + float stride_h = (float)s_h / (float)r_h; + float stride_w = (float)s_w / (float)r_w; + resize_bilinear_letterbox_kernel<<>>( + N, dst, src, d_h, d_w, s_h, s_w, 1.0 / scale, r_h, r_w); +} + +__global__ void resize_bilinear_letterbox_nhwc_to_nchw32_kernel( + int N, float * dst_img, unsigned char * src_img, int dst_h, int dst_w, int src_h, int src_w, + float scale, int letter_bot, int letter_right, float norm) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int C = 3; + int H = dst_h; + int W = dst_w; + int c = 0; + + int w = index % W; + int h = index / W; + + float centroid_h, centroid_w; + centroid_h = scale * (float)(h + 0.5); + centroid_w = scale * (float)(w + 0.5); + + int f00, f01, f10, f11; + + int src_h_idx = lroundf(centroid_h) - 1; + int src_w_idx = lroundf(centroid_w) - 1; + src_h_idx = (src_h_idx < 0) ? 0 : src_h_idx; + src_h_idx = (src_h_idx >= (src_h - 1)) ? src_h - 2 : src_h_idx; + src_w_idx = (src_w_idx < 0) ? 0 : src_w_idx; + src_w_idx = (src_w_idx >= (src_w - 1)) ? src_w - 2 : src_w_idx; + // Unroll + int stride = src_w * C; + for (c = 0; c < C; c++) { + f00 = src_h_idx * stride + src_w_idx * C + c; + f01 = src_h_idx * stride + (src_w_idx + 1) * C + c; + f10 = (src_h_idx + 1) * stride + src_w_idx * C + c; + f11 = (src_h_idx + 1) * stride + (src_w_idx + 1) * C + c; + + float rs = lroundf(lerp2d( + (int)src_img[f00], (int)src_img[f01], (int)src_img[f10], (int)src_img[f11], centroid_h, + centroid_w)); + + // NHCW + int dst_index = w + (W * h) + (W * H * c); + + dst_img[dst_index] = (float)rs; + dst_img[dst_index] = (h >= letter_bot) ? 114.0 : dst_img[dst_index]; + dst_img[dst_index] = (w >= letter_right) ? 114.0 : dst_img[dst_index]; + dst_img[dst_index] *= norm; + } +} + +void resize_bilinear_letterbox_nhwc_to_nchw32_gpu( + float * dst, unsigned char * src, int d_w, int d_h, int d_c, int s_w, int s_h, int s_c, + float norm, cudaStream_t stream) +{ + int N = d_w * d_h; + const float scale = std::min(d_w / (float)s_w, d_h / (float)s_h); + int r_h = scale * s_h; + int r_w = scale * s_w; + float stride_h = (float)s_h / (float)r_h; + float stride_w = (float)s_w / (float)r_w; + + resize_bilinear_letterbox_nhwc_to_nchw32_kernel<<>>( + N, dst, src, d_h, d_w, s_h, s_w, 1.0 / scale, r_h, r_w, norm); +} +} // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index e940679c9c58d..cddaddd1098bb 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,8 @@ #include "cuda_utils/cuda_check_error.hpp" #include "cuda_utils/cuda_unique_ptr.hpp" +#include +#include #include #include @@ -25,16 +27,139 @@ #include #include +namespace +{ +static void trimLeft(std::string & s) +{ + s.erase(s.begin(), find_if(s.begin(), s.end(), [](int ch) { return !isspace(ch); })); +} + +static void trimRight(std::string & s) +{ + s.erase(find_if(s.rbegin(), s.rend(), [](int ch) { return !isspace(ch); }).base(), s.end()); +} + +std::string trim(std::string & s) +{ + trimLeft(s); + trimRight(s); + return s; +} + +bool existFile(const std::string & file_name, bool verbose) +{ + if (!std::experimental::filesystem::exists(std::experimental::filesystem::path(file_name))) { + if (verbose) { + std::cout << "File does not exist : " << file_name << std::endl; + } + return false; + } + return true; +} + +std::vector loadListFromTextFile(const std::string & filename) +{ + assert(existFile(filename, true)); + std::vector list; + + std::ifstream f(filename); + if (!f) { + std::cout << "failed to open " << filename << std::endl; + assert(0); + } + + std::string line; + while (std::getline(f, line)) { + if (line.empty()) { + continue; + } else { + list.push_back(trim(line)); + } + } + + return list; +} + +std::vector loadImageList(const std::string & filename, const std::string & prefix) +{ + std::vector fileList = loadListFromTextFile(filename); + for (auto & file : fileList) { + if (existFile(file, false)) { + continue; + } else { + std::string prefixed = prefix + file; + if (existFile(prefixed, false)) + file = prefixed; + else + std::cerr << "WARNING: couldn't find: " << prefixed << " while loading: " << filename + << std::endl; + } + } + return fileList; +} +} // anonymous namespace + namespace tensorrt_yolox { TrtYoloX::TrtYoloX( const std::string & model_path, const std::string & precision, const int num_class, - const float score_threshold, const float nms_threshold, - [[maybe_unused]] const std::string & cache_dir, const tensorrt_common::BatchConfig & batch_config, - const size_t max_workspace_size) + const float score_threshold, const float nms_threshold, tensorrt_common::BuildConfig build_config, + const bool use_gpu_preprocess, const std::string & calibration_image_list_file, + const double norm_factor, [[maybe_unused]] const std::string & cache_dir, + const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size) { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size); + src_width_ = -1; + src_height_ = -1; + norm_factor_ = norm_factor; + if (precision == "int8") { + if (calibration_image_list_file.empty()) { + throw std::runtime_error( + "calibration_image_list_file should be passed to generate int8 engine."); + } + int max_batch_size = 1; + nvinfer1::Dims input_dims = tensorrt_common::get_input_dims(model_path); + std::vector calibration_images = loadImageList(calibration_image_list_file, ""); + tensorrt_yolox::ImageStream stream(max_batch_size, input_dims, calibration_images); + fs::path calibration_table{model_path}; + std::string calibName = ""; + std::string ext = ""; + if (build_config.calib_type_str == "Entropy") { + ext = "EntropyV2-"; + } else if ( + build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + ext = "Legacy-"; + } else { + ext = "MinMax-"; + } + + ext += "calibration.table"; + calibration_table.replace_extension(ext); + fs::path histogram_table{model_path}; + ext = "histogram.table"; + histogram_table.replace_extension(ext); + + std::unique_ptr calibrator; + if (build_config.calib_type_str == "Entropy") { + calibrator.reset( + new tensorrt_yolox::Int8EntropyCalibrator(stream, calibration_table, norm_factor_)); + + } else if ( + build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + const double quantile = 0.999999; + const double cutoff = 0.999999; + calibrator.reset(new tensorrt_yolox::Int8LegacyCalibrator( + stream, calibration_table, histogram_table, norm_factor_, true, quantile, cutoff)); + } else { + calibrator.reset( + new tensorrt_yolox::Int8MinMaxCalibrator(stream, calibration_table, norm_factor_)); + } + + trt_common_ = std::make_unique( + model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + } else { + trt_common_ = std::make_unique( + model_path, precision, nullptr, batch_config, max_workspace_size, build_config); + } trt_common_->setup(); if (!trt_common_->isInitialized()) { @@ -62,7 +187,7 @@ TrtYoloX::TrtYoloX( default: std::stringstream s; s << "\"" << model_path << "\" is unsupported format"; - std::runtime_error{s.str()}; + throw std::runtime_error{s.str()}; } // GPU memory allocation @@ -77,6 +202,17 @@ TrtYoloX::TrtYoloX( out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); out_prob_d_ = cuda_utils::make_unique(out_elem_num_); out_prob_h_ = cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); + int w = input_dims.d[3]; + int h = input_dims.d[2]; + int sum_tensors = (w / 8) * (h / 8) + (w / 16) * (h / 16) + (w / 32) * (h / 32); + if (sum_tensors == output_dims.d[1]) { + // 3head (8,16,32) + output_strides_ = {8, 16, 32}; + } else { + // 4head (8,16,32.4) + // last is additional head for high resolution outputs + output_strides_ = {8, 16, 32, 4}; + } } else { const auto out_scores_dims = trt_common_->getBindingDimensions(3); max_detections_ = out_scores_dims.d[1]; @@ -86,6 +222,104 @@ TrtYoloX::TrtYoloX( out_scores_d_ = cuda_utils::make_unique(batch_config[2] * max_detections_); out_classes_d_ = cuda_utils::make_unique(batch_config[2] * max_detections_); } + if (use_gpu_preprocess) { + use_gpu_preprocess_ = true; + image_buf_h_ = nullptr; + image_buf_d_ = nullptr; + } else { + use_gpu_preprocess_ = false; + } +} + +void TrtYoloX::initPreprocesBuffer(int width, int height) +{ + // if size of source input has benn changed... + if (src_width_ != -1 || src_height_ != -1) { + if (width != src_width_ || height != src_height_) { + // Free cuda memory to reallocate + if (image_buf_h_) { + image_buf_h_.reset(); + } + if (image_buf_d_) { + image_buf_d_.reset(); + } + } + } + src_width_ = width; + src_height_ = height; + if (use_gpu_preprocess_) { + auto input_dims = trt_common_->getBindingDimensions(0); + if (!image_buf_h_) { + trt_common_->setBindingDimensions(0, input_dims); + scales_.clear(); + } + const float input_height = static_cast(input_dims.d[2]); + const float input_width = static_cast(input_dims.d[3]); + if (!image_buf_h_) { + const float scale = std::min(input_width / width, input_height / height); + scales_.emplace_back(scale); + image_buf_h_ = cuda_utils::make_unique_host( + width * height * 3, cudaHostAllocWriteCombined); + image_buf_d_ = cuda_utils::make_unique(width * height * 3); + } + } +} + +void TrtYoloX::printProfiling(void) +{ + trt_common_->printProfiling(); +} + +void TrtYoloX::preprocessGpu(const std::vector & images) +{ + const auto batch_size = images.size(); + // Currently only supports single batch in cuda preprocessing + assert(batch_size == 1); + auto input_dims = trt_common_->getBindingDimensions(0); + input_dims.d[0] = batch_size; + for (const auto & image : images) { + // if size of source input has been changed... + int width = image.cols; + int height = image.rows; + if (src_width_ != -1 || src_height_ != -1) { + if (width != src_width_ || height != src_height_) { + // Free cuda memory to reallocate + if (image_buf_h_) { + image_buf_h_.reset(); + } + if (image_buf_d_) { + image_buf_d_.reset(); + } + } + } + src_width_ = width; + src_height_ = height; + } + if (!image_buf_h_) { + trt_common_->setBindingDimensions(0, input_dims); + scales_.clear(); + } + const float input_height = static_cast(input_dims.d[2]); + const float input_width = static_cast(input_dims.d[3]); + for (const auto & image : images) { + if (!image_buf_h_) { + const float scale = std::min(input_width / image.cols, input_height / image.rows); + scales_.emplace_back(scale); + image_buf_h_ = cuda_utils::make_unique_host( + image.cols * image.rows * 3, cudaHostAllocWriteCombined); + image_buf_d_ = cuda_utils::make_unique(image.cols * image.rows * 3); + } + // Copy into pinned memory + memcpy(image_buf_h_.get(), &image.data[0], image.cols * image.rows * 3 * sizeof(unsigned char)); + // Copy into device memory + CHECK_CUDA_ERROR(cudaMemcpyAsync( + image_buf_d_.get(), image_buf_h_.get(), image.cols * image.rows * 3 * sizeof(unsigned char), + cudaMemcpyHostToDevice, *stream_)); + // Preprcess on GPU + resize_bilinear_letterbox_nhwc_to_nchw32_gpu( + input_d_.get(), image_buf_d_.get(), input_width, input_height, 3, image.cols, image.rows, 3, + static_cast(norm_factor_), *stream_); + } } void TrtYoloX::preprocess(const std::vector & images) @@ -98,24 +332,39 @@ void TrtYoloX::preprocess(const std::vector & images) const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; scales_.clear(); - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = std::min(input_width / image.cols, input_height / image.rows); - scales_.emplace_back(scale); - const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - const auto bottom = input_height - dst_image.rows; - const auto right = input_width - dst_image.cols; - copyMakeBorder(dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); - dst_images.emplace_back(dst_image); + bool letterbox = true; + if (letterbox) { + for (const auto & image : images) { + cv::Mat dst_image; + const float scale = std::min(input_width / image.cols, input_height / image.rows); + scales_.emplace_back(scale); + const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); + cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + const auto bottom = input_height - dst_image.rows; + const auto right = input_width - dst_image.cols; + copyMakeBorder( + dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); + dst_images.emplace_back(dst_image); + } + } else { + for (const auto & image : images) { + cv::Mat dst_image; + const float scale = -1.0; + scales_.emplace_back(scale); + const auto scale_size = cv::Size(input_width, input_height); + cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + dst_images.emplace_back(dst_image); + } } - const auto chw_images = - cv::dnn::blobFromImages(dst_images, 1.0, cv::Size(), cv::Scalar(), false, false, CV_32F); + const auto chw_images = cv::dnn::blobFromImages( + dst_images, norm_factor_, cv::Size(), cv::Scalar(), false, false, CV_32F); const auto data_length = chw_images.total(); input_h_.reserve(data_length); const auto flat = chw_images.reshape(1, data_length); input_h_ = chw_images.isContinuous() ? flat : flat.clone(); + CHECK_CUDA_ERROR(cudaMemcpy( + input_d_.get(), input_h_.data(), input_h_.size() * sizeof(float), cudaMemcpyHostToDevice)); } bool TrtYoloX::doInference(const std::vector & images, ObjectArrays & objects) @@ -124,10 +373,11 @@ bool TrtYoloX::doInference(const std::vector & images, ObjectArrays & o return false; } - preprocess(images); - - CHECK_CUDA_ERROR(cudaMemcpy( - input_d_.get(), input_h_.data(), input_h_.size() * sizeof(float), cudaMemcpyHostToDevice)); + if (use_gpu_preprocess_) { + preprocessGpu(images); + } else { + preprocess(images); + } if (needs_output_decode_) { return feedforwardAndDecode(images, objects); @@ -219,9 +469,8 @@ void TrtYoloX::decodeOutputs( auto input_dims = trt_common_->getBindingDimensions(0); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); - std::vector strides = {8, 16, 32}; std::vector grid_strides; - generateGridsAndStride(input_width, input_height, strides, grid_strides); + generateGridsAndStride(input_width, input_height, output_strides_, grid_strides); generateYoloxProposals(grid_strides, prob, score_threshold_, proposals); qsortDescentInplace(proposals); @@ -232,15 +481,24 @@ void TrtYoloX::decodeOutputs( int count = static_cast(picked.size()); objects.resize(count); + float scale_x = input_width / static_cast(img_size.width); + float scale_y = input_height / static_cast(img_size.height); for (int i = 0; i < count; i++) { objects[i] = proposals[picked[i]]; + float x0, y0, x1, y1; // adjust offset to original unpadded - float x0 = (objects[i].x_offset) / scale; - float y0 = (objects[i].y_offset) / scale; - float x1 = (objects[i].x_offset + objects[i].width) / scale; - float y1 = (objects[i].y_offset + objects[i].height) / scale; - + if (scale == -1.0) { + x0 = (objects[i].x_offset) / scale_x; + y0 = (objects[i].y_offset) / scale_y; + x1 = (objects[i].x_offset + objects[i].width) / scale_x; + y1 = (objects[i].y_offset + objects[i].height) / scale_y; + } else { + x0 = (objects[i].x_offset) / scale; + y0 = (objects[i].y_offset) / scale; + x1 = (objects[i].x_offset + objects[i].width) / scale; + y1 = (objects[i].y_offset + objects[i].height) / scale; + } // clip x0 = std::clamp(x0, 0.f, static_cast(img_size.width - 1)); y0 = std::clamp(y0, 0.f, static_cast(img_size.height - 1)); @@ -255,7 +513,7 @@ void TrtYoloX::decodeOutputs( } void TrtYoloX::generateGridsAndStride( - const int target_w, const int target_h, std::vector & strides, + const int target_w, const int target_h, const std::vector & strides, std::vector & grid_strides) const { for (auto stride : strides) { @@ -287,10 +545,12 @@ void TrtYoloX::generateYoloxProposals( // (i.e., `decode_in_inference` should be False) float x_center = (feat_blob[basic_pos + 0] + grid0) * stride; float y_center = (feat_blob[basic_pos + 1] + grid1) * stride; - float w = exp(feat_blob[basic_pos + 2]) * stride; - float h = exp(feat_blob[basic_pos + 3]) * stride; - float x0 = x_center - w * 0.5f; - float y0 = y_center - h * 0.5f; + + // exp is complex for embedded processors + // float w = exp(feat_blob[basic_pos + 2]) * stride; + // float h = exp(feat_blob[basic_pos + 3]) * stride; + // float x0 = x_center - w * 0.5f; + // float y0 = y_center - h * 0.5f; float box_objectness = feat_blob[basic_pos + 4]; for (int class_idx = 0; class_idx < num_class_; class_idx++) { @@ -298,6 +558,11 @@ void TrtYoloX::generateYoloxProposals( float box_prob = box_objectness * box_cls_score; if (box_prob > prob_threshold) { Object obj; + // On-demand applying for exp + float w = exp(feat_blob[basic_pos + 2]) * stride; + float h = exp(feat_blob[basic_pos + 3]) * stride; + float x0 = x_center - w * 0.5f; + float y0 = y_center - h * 0.5f; obj.x_offset = x0; obj.y_offset = y0; obj.height = h; diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index 8d03bfaee1877..65307ff28acde 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -32,23 +32,75 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) using std::placeholders::_1; using std::chrono_literals::operator""ms; - std::string model_path = declare_parameter("model_path", ""); - std::string label_path = declare_parameter("label_path", ""); - std::string precision = declare_parameter("precision", "fp32"); - // Objects with a score lower than this value will be ignored. - // This threshold will be ignored if specified model contains EfficientNMS_TRT module in it - float score_threshold = declare_parameter("score_threshold", 0.3); - // Detection results will be ignored if IoU over this value. - // This threshold will be ignored if specified model contains EfficientNMS_TRT module in it - float nms_threshold = declare_parameter("nms_threshold", 0.7); + auto declare_parameter_with_description = + [this](std::string name, auto default_val, std::string description = "") { + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + param_desc.description = description; + return this->declare_parameter(name, default_val, param_desc); + }; + + std::string model_path = + declare_parameter_with_description("model_path", "", "The onnx file name for YOLOX model"); + std::string label_path = declare_parameter_with_description( + "label_path", "", + "The label file that consists of label name texts for detected object categories"); + std::string precision = declare_parameter_with_description( + "precision", "fp32", + "operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]"); + float score_threshold = declare_parameter_with_description( + "score_threshold", 0.3, + ("Objects with a score lower than this value will be ignored. " + "This threshold will be ignored if specified model contains EfficientNMS_TRT module in it")); + float nms_threshold = declare_parameter_with_description( + "nms_threshold", 0.7, + ("Detection results will be ignored if IoU over this value. " + "This threshold will be ignored if specified model contains EfficientNMS_TRT module in it")); + std::string calibration_algorithm = declare_parameter_with_description( + "calibration_algorithm", "MinMax", + ("Calibration algorithm to be used for quantization when precision==int8. " + "Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]")); + int dla_core_id = declare_parameter_with_description( + "dla_core_id", -1, + "If positive ID value is specified, the node assign inference task to the DLA core"); + bool quantize_first_layer = declare_parameter_with_description( + "quantize_first_layer", false, + ("If true, set the operating precision for the first (input) layer to be fp16. " + "This option is valid only when precision==int8")); + bool quantize_last_layer = declare_parameter_with_description( + "quantize_last_layer", false, + ("If true, set the operating precision for the last (output) layer to be fp16. " + "This option is valid only when precision==int8")); + bool profile_per_layer = declare_parameter_with_description( + "profile_per_layer", false, + ("If true, profiler function will be enabled. " + "Since the profile function may affect execution speed, it is recommended " + "to set this flag true only for development purpose.")); + double clip_value = declare_parameter_with_description( + "clip_value", 0.0, + ("If positive value is specified, " + "the value of each layer output will be clipped between [0.0, clip_value]. " + "This option is valid only when precision==int8 and used to manually specify " + "the dynamic range instead of using any calibration")); + bool preprocess_on_gpu = declare_parameter_with_description( + "preprocess_on_gpu", true, "If true, pre-processing is performed on GPU"); + std::string calibration_image_list_path = declare_parameter_with_description( + "calibration_image_list_path", "", + ("Path to a file which contains path to images." + "Those images will be used for int8 quantization.")); if (!readLabelFile(label_path)) { RCLCPP_ERROR(this->get_logger(), "Could not find label file"); rclcpp::shutdown(); } replaceLabelMap(); + + tensorrt_common::BuildConfig build_config( + calibration_algorithm, dla_core_id, quantize_first_layer, quantize_last_layer, + profile_per_layer, clip_value); + trt_yolox_ = std::make_unique( - model_path, precision, label_map_.size(), score_threshold, nms_threshold); + model_path, precision, label_map_.size(), score_threshold, nms_threshold, build_config, + preprocess_on_gpu, calibration_image_list_path); timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrtYoloXNode::onConnect, this)); diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp index a667d39248bba..d75ad5e91d46a 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp @@ -25,7 +25,11 @@ #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp index 27548c9bd92c9..ffc7bf18fbf57 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp @@ -24,7 +24,11 @@ #include +#if __has_include() +#include +#else #include +#endif #include diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp index 8216d1fd2edab..8c0cbb015e9be 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -29,7 +29,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include #include diff --git a/perception/traffic_light_classifier/package.xml b/perception/traffic_light_classifier/package.xml index 923de8c43bd34..2f7b07962a23b 100644 --- a/perception/traffic_light_classifier/package.xml +++ b/perception/traffic_light_classifier/package.xml @@ -9,10 +9,9 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake wget - autoware_cmake - autoware_auto_perception_msgs cv_bridge image_transport diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index 7cce3d78c4b0c..941e0596bf061 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -44,7 +44,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include #include diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 46220572ae9a3..3b19413e5ecd6 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_perception_msgs diff --git a/perception/traffic_light_selector/package.xml b/perception/traffic_light_selector/package.xml index 62374ddb7c0bf..75796ac3cd710 100644 --- a/perception/traffic_light_selector/package.xml +++ b/perception/traffic_light_selector/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_perception_msgs diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index 8e91e36fdda7a..76a0e3d75c20f 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -119,6 +119,16 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ### ssd ### + cuda_add_library(gather_topk_plugin SHARED + lib/src/plugins/gather_topk_kernel.cu + lib/src/plugins/gather_topk.cpp + ) + + cuda_add_library(grid_priors_plugin SHARED + lib/src/plugins/grid_priors_kernel.cu + lib/src/plugins/grid_priors.cpp + ) + ament_auto_add_library(ssd SHARED lib/src/trt_ssd.cpp ) @@ -127,6 +137,14 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/include ) + target_include_directories(gather_topk_plugin PUBLIC + lib/include + ) + + target_include_directories(grid_priors_plugin PUBLIC + lib/include + ) + target_link_libraries(ssd ${NVINFER} ${NVONNXPARSER} @@ -134,6 +152,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} ${CUDNN_LIBRARY} + gather_topk_plugin + grid_priors_plugin ) ament_auto_add_library(traffic_light_ssd_fine_detector_nodelet SHARED @@ -144,6 +164,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${OpenCV_LIB} stdc++fs ssd + gather_topk_plugin + grid_priors_plugin ) rclcpp_components_register_node(traffic_light_ssd_fine_detector_nodelet @@ -156,6 +178,15 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) launch ) + install( + TARGETS + gather_topk_plugin + grid_priors_plugin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + ) + else() message(STATUS "TrafficLightSSDFineDetector won't be built, CUDA and/or TensorRT were not found.") # to avoid launch file missing without a gpu diff --git a/perception/traffic_light_ssd_fine_detector/README.md b/perception/traffic_light_ssd_fine_detector/README.md index cf32c027e6af4..01a30fac88fe4 100644 --- a/perception/traffic_light_ssd_fine_detector/README.md +++ b/perception/traffic_light_ssd_fine_detector/README.md @@ -6,18 +6,90 @@ It is a package for traffic light detection using MobileNetV2 and SSDLite. ## Training Information +_NOTE_: + +- Currently, Autoware supports SSD trained both [pytorch-ssd](https://github.com/qfgaohao/pytorch-ssd) and [mmdetection](https://github.com/open-mmlab/mmdetection.git). + - Please specify either `pytorch` or `mmdetection` in `dnn_header_type`. +- Note that, tensor names of your onnx model conform the following. + - Input tensor: `input` + - Output box tensor: `boxes`. + - Output score tensor: `scores`. + ### Pretrained Model The model is based on [pytorch-ssd](https://github.com/qfgaohao/pytorch-ssd) and the pretrained model could be downloaded from [here](https://drive.google.com/file/d/1puI6ltKZKJ4RoiCO-ypivzEysHaDVBsa/view). ### Training Data -The model was fine-tuned on 1750 TierIV internal images of Japanese traffic lights. +The model was fine-tuned on 1750 TIER IV internal images of Japanese traffic lights. ### Trained Onnx model - +### Customization of CNN model + +In order to train models and export onnx model, we recommend [open-mmlab/mmdetection](https://github.com/open-mmlab/mmdetection.git). +Please follow the [official document](https://mmdetection.readthedocs.io/en/latest/) to install and experiment with mmdetection. If you get into troubles, [FAQ page](https://mmdetection.readthedocs.io/en/stable/notes/faq.html) would help you. + +The following steps are example of a quick-start. + +#### step 0. Install [MMCV](https://github.com/open-mmlab/mmcv.git) and [MIM](https://github.com/open-mmlab/mim.git) + +_NOTE_ : + +- First of all, install [PyTorch](https://pytorch.org/) suitable for your CUDA version (CUDA11.6 is supported in Autoware). +- Our tested library versions are following. If the scripts as shown below would be old, please update to be suited to your version. + - MMCV == 1.x + - MMDetection == 2.x + - MMDeploy == 0.x + - MIM == 0.3.x + +In order to install mmcv suitable for your CUDA version, install it specifying a url. + +```shell +# Install mim +$ pip install -U openmim + +# Install mmcv on a machine with CUDA11.6 and PyTorch1.13.0 +$ pip install mmcv-full -f https://download.openmmlab.com/mmcv/dist/cu116/torch1.13/index.html +``` + +#### step 1. Install MMDetection + +You can install mmdetection as a Python package or from source. + +```shell +# As a Python package +$ pip install mmdet + +# From source +$ git clone https://github.com/open-mmlab/mmdetection.git +$ cd mmdetection +$ pip install -v -e . +``` + +#### step 2. Train your model + +Train model with your experiment configuration file. For the details of config file, see [here](https://mmdetection.readthedocs.io/en/latest/user_guides/config.html). + +```shell +# [] is optional, you can start training from pre-trained checkpoint +$ mim train mmdet YOUR_CONFIG.py [--resume-from YOUR_CHECKPOINT.pth] +``` + +#### step 3. Export onnx model + +In exporting onnx, use `mmdetection/tools/deployment/pytorch2onnx.py` or [open-mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy.git). +_NOTE_: + +- Currently, autoware does not support TensorRT plugin for NMS defined by open-mmlab. Therefore, **please deploy onnx model excluding NMS layer**. + +```shell +cd ~/mmdetection/tools/deployment +python3 pytorch2onnx.py YOUR_CONFIG.py ... +``` + ## Inner-workings / Algorithms Based on the camera image and the global ROI array detected by `map_based_detection` node, a CNN-based detection method enables highly accurate traffic light detection. @@ -54,6 +126,7 @@ Based on the camera image and the global ROI array detected by `map_based_detect | ------------------ | ------ | ------------------------------ | -------------------------------------------------------------------- | | `onnx_file` | string | "./data/mb2-ssd-lite-tlr.onnx" | The onnx file name for yolo model | | `label_file` | string | "./data/voc_labels_tl.txt" | The label file with label names for detected objects written on it | +| `dnn_header_type` | string | "pytorch" | Name of DNN trained toolbox: "pytorch" or "mmdetection" | | `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | | `max_batch_size` | int | 8 | The size of the batch processed at one time by inference by TensorRT | | `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | diff --git a/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp b/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp index 0608bdb5b7dca..a2fa16de01a25 100644 --- a/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp +++ b/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -27,7 +27,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include #include @@ -97,11 +101,12 @@ class TrafficLightSSDFineDetectorNodelet : public rclcpp::Node double score_thresh_; int tlr_id_; - int channel_; - int width_; - int height_; + ssd::Shape input_shape_; int class_num_; int detection_per_class_; + std::optional box_dims_; + std::optional score_dims_; + std::string dnn_header_type_; std::vector mean_; std::vector std_; diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml index 1dc23f7c654c6..a4d61b774652a 100644 --- a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -2,6 +2,7 @@ + @@ -20,6 +21,7 @@ + diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp index 79984ecba8e3d..3f317c613f23e 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,7 +22,10 @@ #define CUDA_UTILS_HPP_ #include <./cuda_runtime_api.h> +#include +#include +#include #include #include #include @@ -30,6 +33,20 @@ #define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) +#define CUDA_1D_KERNEL_LOOP(i, n) \ + for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < (n); i += blockDim.x * gridDim.x) + +#define THREADS_PER_BLOCK 512 + +#define DIVUP(m, n) ((m) / (n) + ((m) % (n) > 0)) + +inline int GET_BLOCKS(const int N) +{ + int optimal_block_num = DIVUP(N, THREADS_PER_BLOCK); + constexpr int max_block_num = 4096; + return std::min(optimal_block_num, max_block_num); +} + namespace cuda { void check_error(const ::cudaError_t e, decltype(__FILE__) f, decltype(__LINE__) n) @@ -50,7 +67,8 @@ template using unique_ptr = std::unique_ptr; // auto array = cuda::make_unique(n); -// ::cudaMemcpy(array.get(), src_array, sizeof(float)*n, ::cudaMemcpyHostToDevice); +// ::cudaMemcpy(array.get(), src_array, sizeof(float)*n, +// ::cudaMemcpyHostToDevice); template typename std::enable_if::value, cuda::unique_ptr>::type make_unique( const std::size_t n) @@ -62,7 +80,8 @@ typename std::enable_if::value, cuda::unique_ptr>::type make } // auto value = cuda::make_unique(); -// ::cudaMemcpy(value.get(), src_value, sizeof(my_class), ::cudaMemcpyHostToDevice); +// ::cudaMemcpy(value.get(), src_value, sizeof(my_class), +// ::cudaMemcpyHostToDevice); template cuda::unique_ptr make_unique() { diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk.hpp new file mode 100644 index 0000000000000..5a71e17081213 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk.hpp @@ -0,0 +1,102 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#ifndef GATHER_TOPK_HPP_ +#define GATHER_TOPK_HPP_ + +#include "trt_plugin_helper.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace ssd +{ +class GatherTopk : public nvinfer1::IPluginV2DynamicExt +{ +private: + const std::string mLayerName; + std::string mNamespace; + +public: + explicit GatherTopk(const std::string & name); + GatherTopk(const std::string & name, const void * data, size_t length); + GatherTopk() = delete; + + // IPluginV2 methods + const char * getPluginVersion() const noexcept override; + const char * getPluginType() const noexcept override; + int initialize() noexcept override; + void terminate() noexcept override; + void destroy() noexcept override; + void setPluginNamespace(const char * pluginNamespace) noexcept override; + const char * getPluginNamespace() const noexcept override; + int getNbOutputs() const noexcept override; + size_t getSerializationSize() const noexcept override; + void serialize(void * buffer) const noexcept override; + + // IPluginV2Ext methods + nvinfer1::DataType getOutputDataType( + int index, const nvinfer1::DataType * inputTypes, int nbInputs) const noexcept override; + + // IPluginV2DynamicExt methods + nvinfer1::IPluginV2DynamicExt * clone() const noexcept override; + nvinfer1::DimsExprs getOutputDimensions( + int outputIndex, const nvinfer1::DimsExprs * inputs, int nbInputs, + nvinfer1::IExprBuilder & exprBuilder) noexcept override; + bool supportsFormatCombination( + int pos, const nvinfer1::PluginTensorDesc * ioDesc, int nbInputs, + int nbOutputs) noexcept override; + void configurePlugin( + const nvinfer1::DynamicPluginTensorDesc * inputs, int nbInputs, + const nvinfer1::DynamicPluginTensorDesc * outputs, int nbOutputs) noexcept override; + size_t getWorkspaceSize( + const nvinfer1::PluginTensorDesc *, int, const nvinfer1::PluginTensorDesc *, + int) const noexcept override; + int enqueue( + const nvinfer1::PluginTensorDesc *, const nvinfer1::PluginTensorDesc *, const void * const *, + void * const *, void *, cudaStream_t) noexcept override; +}; // class GatherTopk + +class GatherTopkCreator : public nvinfer1::IPluginCreator +{ +private: + nvinfer1::PluginFieldCollection mFC; + std::vector mPluginAttributes; + std::string mNamespace; + +public: + GatherTopkCreator(); + const char * getPluginVersion() const noexcept override; + const nvinfer1::PluginFieldCollection * getFieldNames() noexcept override; + void setPluginNamespace(const char * pluginNamespace) noexcept override; + const char * getPluginNamespace() const noexcept override; + const char * getPluginName() const noexcept override; + nvinfer1::IPluginV2 * createPlugin( + const char * name, const nvinfer1::PluginFieldCollection * fc) noexcept override; + nvinfer1::IPluginV2 * deserializePlugin( + const char * name, const void * serialData, size_t serialLength) noexcept override; +}; // class GatherTopkCreator + +REGISTER_TENSORRT_PLUGIN(GatherTopkCreator); + +} // namespace ssd + +#endif // GATHER_TOPK_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk_kernel.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk_kernel.hpp new file mode 100644 index 0000000000000..d9cc9fa533df6 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk_kernel.hpp @@ -0,0 +1,30 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#ifndef GATHER_TOPK_KERNEL_HPP_ +#define GATHER_TOPK_KERNEL_HPP_ + +#include + +namespace ssd +{ +template +void gather_topk_impl( + const scalar_t * input, const int * indices, const int * dims, int nbDims, + const int * indices_dims, int indices_nbDims, scalar_t * output, cudaStream_t stream); +} // namespace ssd + +#endif // GATHER_TOPK_KERNEL_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors.hpp new file mode 100644 index 0000000000000..0c0741c28e0db --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors.hpp @@ -0,0 +1,104 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#ifndef GRID_PRIORS_HPP_ +#define GRID_PRIORS_HPP_ + +#include "trt_plugin_helper.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace ssd +{ +class GridPriors : public nvinfer1::IPluginV2DynamicExt +{ +private: + const std::string mLayerName; + std::string mNamespace; + nvinfer1::Dims mStride; + cublasHandle_t m_cublas_handle; + +public: + explicit GridPriors(const std::string & name, const nvinfer1::Dims & stride); + GridPriors(const std::string & name, const void *, size_t length); + GridPriors() = delete; + + // IPluginV2 methods + const char * getPluginVersion() const noexcept override; + const char * getPluginType() const noexcept override; + int initialize() noexcept override; + void terminate() noexcept override; + void destroy() noexcept override; + void setPluginNamespace(const char * pluginNamespace) noexcept override; + const char * getPluginNamespace() const noexcept override; + int getNbOutputs() const noexcept override; + size_t getSerializationSize() const noexcept override; + void serialize(void * buffer) const noexcept override; + + // IPluginV2Ext methods + nvinfer1::DataType getOutputDataType( + int index, const nvinfer1::DataType * inputTypes, int nbInputs) const noexcept override; + + // IPluginV2DynamicExt methods + nvinfer1::IPluginV2DynamicExt * clone() const noexcept override; + nvinfer1::DimsExprs getOutputDimensions( + int outputIndex, const nvinfer1::DimsExprs * inputs, int nbInputs, + nvinfer1::IExprBuilder & exprBuilder) noexcept override; + bool supportsFormatCombination( + int pos, const nvinfer1::PluginTensorDesc * ioDesc, int nbInputs, + int nbOutputs) noexcept override; + void configurePlugin( + const nvinfer1::DynamicPluginTensorDesc * inputs, int nbInputs, + const nvinfer1::DynamicPluginTensorDesc * outputs, int nbOutputs) noexcept override; + size_t getWorkspaceSize( + const nvinfer1::PluginTensorDesc *, int, const nvinfer1::PluginTensorDesc *, + int) const noexcept override; + int enqueue( + const nvinfer1::PluginTensorDesc *, const nvinfer1::PluginTensorDesc *, const void * const *, + void * const *, void *, cudaStream_t) noexcept override; +}; // class GridPriors + +class GridPriorsCreator : public nvinfer1::IPluginCreator +{ +private: + nvinfer1::PluginFieldCollection mFC; + std::vector mPluginAttributes; + std::string mNamespace; + +public: + GridPriorsCreator(); + const char * getPluginVersion() const noexcept override; + const nvinfer1::PluginFieldCollection * getFieldNames() noexcept override; + void setPluginNamespace(const char * pluginNamespace) noexcept override; + const char * getPluginNamespace() const noexcept override; + const char * getPluginName() const noexcept override; + nvinfer1::IPluginV2 * createPlugin( + const char * name, const nvinfer1::PluginFieldCollection * fc) noexcept override; + nvinfer1::IPluginV2 * deserializePlugin( + const char * name, const void * serialData, size_t serialLength) noexcept override; +}; // class GridPriorsCreator + +REGISTER_TENSORRT_PLUGIN(GridPriorsCreator); + +} // namespace ssd + +#endif // GRID_PRIORS_HPP_ diff --git a/perception/compare_map_segmentation/src/multi_voxel_grid_map_update.cpp b/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors_kernel.hpp similarity index 55% rename from perception/compare_map_segmentation/src/multi_voxel_grid_map_update.cpp rename to perception/traffic_light_ssd_fine_detector/lib/include/grid_priors_kernel.hpp index 0efdf9a8a1850..04b4d0142d93b 100644 --- a/perception/compare_map_segmentation/src/multi_voxel_grid_map_update.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors_kernel.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Autoware Foundation +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,6 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +// Copyright (c) OpenMMLab. All rights reserved. -template class compare_map_segmentation::MultiVoxelGrid; +#ifndef GRID_PRIORS_KERNEL_HPP_ +#define GRID_PRIORS_KERNEL_HPP_ + +#include + +namespace ssd +{ +template +void grid_priors_impl( + const scalar_t * base_anchor, scalar_t * output, int num_base_anchors, int feat_w, int feat_h, + int stride_w, int stride_h, cudaStream_t stream); +} // namespace ssd + +#endif // GRID_PRIORS_KERNEL_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp new file mode 100644 index 0000000000000..7ca28498ed12d --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp @@ -0,0 +1,67 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#ifndef TRT_PLUGIN_HELPER_HPP_ +#define TRT_PLUGIN_HELPER_HPP_ + +#include +#include + +#include +#include + +cudnnStatus_t convert_trt2cudnn_dtype(nvinfer1::DataType trt_dtype, cudnnDataType_t * cudnn_dtype); + +enum pluginStatus_t { + STATUS_SUCCESS = 0, + STATUS_FAILURE = 1, + STATUS_BAD_PARAM = 2, + STATUS_NOT_SUPPORTED = 3, + STATUS_NOT_INITIALIZED = 4 +}; // enum pluginStatus_t + +#define ASSERT(assertion) \ + { \ + if (!assertion) { \ + std::cerr << "#assertion" << __FILE__ << ", " << __LINE__ << std::endl; \ + abort(); \ + } \ + } + +#define CUASSERT(status_) \ + { \ + auto s_ = status; \ + if (s_ != cudaSuccess) { \ + std::cerr << __FILE__ << ", " << __LINE__ << ", " << s_ << ", " << cudaGetErrorString(s_) \ + << std::endl; \ + } \ + } + +#define CUBLASASSERT(status_) \ + { \ + auto s_ = status_; \ + if (s_ != CUBLAS_STATUS_SUCCESS) { \ + std::cerr << __FILE__ << ", " << __LINE__ << ", " << s_ << std::endl; \ + } \ + } + +#define CUERRORMSG(status_) \ + { \ + auto s_ = status_; \ + if (s_ != 0) std::cerr << __FILE__ << ", " << __LINE__ << ", " << s_ << std::endl; \ + } + +#endif // TRT_PLUGIN_HELPER_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_serialize.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_serialize.hpp new file mode 100644 index 0000000000000..bfea3bbc6eadb --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_serialize.hpp @@ -0,0 +1,127 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#ifndef TRT_SERIALIZE_HPP_ +#define TRT_SERIALIZE_HPP_ + +#include +#include +#include +#include + +template +inline void serialize_value(void ** buffer, T const & value); + +template +inline void deserialize_value(void const ** buffer, size_t * buffer_size, T * value); + +namespace +{ +template +struct Serializer +{ +}; // struct Serializer + +template +struct Serializer< + T, typename std::enable_if< + std::is_arithmetic::value || std::is_enum::value || std::is_pod::value>::type> +{ + static size_t serialized_size(T const &) { return sizeof(T); } + static void serialize(void ** buffer, T const & value) + { + ::memcpy(*buffer, &value, sizeof(T)); + reinterpret_cast(*buffer) += sizeof(T); + } + static void deserialize(void const ** buffer, size_t * buffer_size, T * value) + { + assert(sizeof(T) <= *buffer_size); + ::memcpy(value, *buffer, sizeof(T)); + reinterpret_cast(*buffer) += sizeof(T); + *buffer_size -= sizeof(T); + } +}; // struct Serializer + +template <> +struct Serializer +{ + static size_t serialized_size(const char * value) { return strlen(value) + 1; } + static void serialize(void ** buffer, const char * value) + { + ::memcpy(static_cast(*buffer), value, sizeof(char)); + reinterpret_cast(*buffer) += strlen(value) + 1; + } + static void deserialize(void const ** buffer, size_t * buffer_size, const char ** value) + { + *value = static_cast(*buffer); + size_t data_size = strnlen(*value, *buffer_size) + 1; + assert(data_size <= *buffer_size); + reinterpret_cast(*buffer) += data_size; + *buffer_size -= data_size; + } +}; // struct Serializer + +template +struct Serializer< + std::vector, + typename std::enable_if< + std::is_arithmetic::value || std::is_enum::value || std::is_pod::value>::type> +{ + static size_t serialized_size(std::vector const & value) + { + return sizeof(value.size()) + value.size() * sizeof(T); + } + static void serialize(void ** buffer, std::vector const & value) + { + serialize_value(buffer, value.size()); + size_t nbyte = value.size() * sizeof(T); + ::memcpy(*buffer, value.data(), nbyte); + reinterpret_cast(*buffer) += nbyte; + } + static void deserialize(void const ** buffer, size_t * buffer_size, std::vector * value) + { + size_t size; + deserialize_value(buffer, buffer_size, &size); + value->resize(size); + size_t nbyte = value->size() * sizeof(T); + assert(nbyte <= *buffer_size); + ::memcpy(value->data(), *buffer, nbyte); + reinterpret_cast(*buffer) += nbyte; + *buffer_size -= nbyte; + } +}; // struct Serializer + +} // namespace + +template +inline size_t serialized_size(T const & value) +{ + return Serializer::serialized_size(value); +} + +template +inline void serialize_value(void ** buffer, T const & value) +{ + return Serializer::serialize(buffer, value); +} + +template +inline void deserialize_value(void const ** buffer, size_t * buffer_size, T * value) +{ + return Serializer::deserialize(buffer, buffer_size, value); +} + +#endif // TRT_SERIALIZE_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp index cfb6c8a6d5754..52d411c253fe5 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -55,6 +55,20 @@ class Logger : public nvinfer1::ILogger bool verbose_{false}; }; +struct Shape +{ + int channel, width, height; + inline int size() const { return channel * width * height; } + inline int area() const { return width * height; } +}; + +class Dims2 : public nvinfer1::Dims2 +{ +public: + Dims2(const int32_t d0, const int32_t d1) : nvinfer1::Dims2(d0, d1) {} + inline int size() const { return d[0] * d[1]; } +}; + class Net { public: @@ -75,15 +89,37 @@ class Net void infer(std::vector & buffers, const int batch_size); // Get (c, h, w) size of the fixed input - std::vector getInputSize(); + inline Shape getInputShape() const + { + auto dims = getTensorShape("input"); + return {dims.d[1], dims.d[2], dims.d[3]}; + } - std::vector getOutputScoreSize(); + // Get output dimensions by name + inline Dims2 getOutputDimensions(const char * name) const + { + auto dims = getTensorShape(name); + return Dims2(dims.d[1], dims.d[2]); + } // Get max allowed batch size - int getMaxBatchSize(); + inline int getMaxBatchSize() const + { + return engine_->getProfileDimensions(0, 0, nvinfer1::OptProfileSelector::kMAX).d[0]; + } // Get max number of detections - int getMaxDetections(); + inline int getMaxDetections() const { return getTensorShape("boxes").d[1]; } + + // Get specified name of tensor shape + inline nvinfer1::Dims getTensorShape(const char * name) const + { +#if (NV_TENSORRT_MAJOR * 10000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 80500 + return engine_->getTensorShape(name); +#else + return engine_->getBindingDimensions(engine_->getBindingIndex(name)); +#endif + } private: unique_ptr runtime_ = nullptr; diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/plugins/gather_topk.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/gather_topk.cpp new file mode 100644 index 0000000000000..f083733b4d5b4 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/gather_topk.cpp @@ -0,0 +1,236 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#include "gather_topk.hpp" + +#include "gather_topk_kernel.hpp" +#include "trt_serialize.hpp" + +#include +#include + +#include + +namespace ssd +{ +namespace +{ +static const char * PLUGIN_VERSION{"1"}; +static const char * PLUGIN_NAME{"GatherTopk"}; +} // namespace + +GatherTopk::GatherTopk(const std::string & name) : mLayerName(name) +{ +} +GatherTopk::GatherTopk(const std::string & name, const void *, size_t) : mLayerName(name) +{ +} + +// IPluginV2 methods +const char * GatherTopk::getPluginVersion() const noexcept +{ + return PLUGIN_VERSION; +} + +const char * GatherTopk::getPluginType() const noexcept +{ + return PLUGIN_NAME; +} + +int GatherTopk::initialize() noexcept +{ + return STATUS_SUCCESS; +} + +void GatherTopk::terminate() noexcept +{ +} + +void GatherTopk::destroy() noexcept +{ + delete this; +} + +void GatherTopk::setPluginNamespace(const char * pluginNamespace) noexcept +{ + mNamespace = pluginNamespace; +} + +const char * GatherTopk::getPluginNamespace() const noexcept +{ + return mNamespace.c_str(); +} + +int GatherTopk::getNbOutputs() const noexcept +{ + return 1; +} + +size_t GatherTopk::getSerializationSize() const noexcept +{ + return 0; +} + +void GatherTopk::serialize(void *) const noexcept +{ +} + +// IPluginV2Ext methods +nvinfer1::DataType GatherTopk::getOutputDataType( + int, const nvinfer1::DataType * inputTypes, int) const noexcept +{ + return inputTypes[0]; +} + +// IPluginV2DynamicExt methods +nvinfer1::IPluginV2DynamicExt * GatherTopk::clone() const noexcept +{ + GatherTopk * plugin = new GatherTopk(mLayerName); + plugin->setPluginNamespace(getPluginNamespace()); + return plugin; +} + +nvinfer1::DimsExprs GatherTopk::getOutputDimensions( + int, const nvinfer1::DimsExprs * inputs, int, nvinfer1::IExprBuilder &) noexcept +{ + assert(inputs[1].nbDims <= inputs[0].nbDims); + nvinfer1::DimsExprs ret; + ret.nbDims = inputs[0].nbDims; + for (int i = 0; i < inputs[1].nbDims; ++i) { + ret.d[i] = inputs[1].d[i]; + } + for (int i = inputs[1].nbDims; i < inputs[0].nbDims; ++i) { + ret.d[i] = inputs[0].d[i]; + } + return ret; +} + +bool GatherTopk::supportsFormatCombination( + int pos, const nvinfer1::PluginTensorDesc * ioDesc, int, int) noexcept +{ + switch (pos) { + case 0: + // data + return (ioDesc[pos].type == nvinfer1::DataType::kFLOAT && + ioDesc[pos].format == nvinfer1::TensorFormat::kLINEAR) || + (ioDesc[pos].type == nvinfer1::DataType::kINT32 && + ioDesc[pos].format == nvinfer1::TensorFormat::kLINEAR); + case 1: + // indices + return ioDesc[pos].type == nvinfer1::DataType::kINT32 && + ioDesc[pos].format == nvinfer1::TensorFormat::kLINEAR; + case 2: + // output + return ioDesc[pos].type == ioDesc[0].type && ioDesc[pos].format == ioDesc[0].format; + default: + return true; + } +} + +void GatherTopk::configurePlugin( + const nvinfer1::DynamicPluginTensorDesc *, int, const nvinfer1::DynamicPluginTensorDesc *, + int) noexcept +{ +} + +size_t GatherTopk::getWorkspaceSize( + const nvinfer1::PluginTensorDesc *, int, const nvinfer1::PluginTensorDesc *, int) const noexcept +{ + return 0; +} + +int GatherTopk::enqueue( + const nvinfer1::PluginTensorDesc * inputDesc, const nvinfer1::PluginTensorDesc *, + const void * const * inputs, void * const * outputs, void *, cudaStream_t stream) noexcept +{ + const int * dims = &(inputDesc[0].dims.d[0]); + const int * dims_indices = &(inputDesc[1].dims.d[0]); + int nbDims = inputDesc[0].dims.nbDims; + int nbDims_index = inputDesc[1].dims.nbDims; + + const void * data = inputs[0]; + const void * indices = inputs[1]; + void * output = outputs[0]; + + auto data_type = inputDesc[0].type; + + switch (data_type) { + case nvinfer1::DataType::kFLOAT: + gather_topk_impl( + reinterpret_cast(data), reinterpret_cast(indices), dims, nbDims, + dims_indices, nbDims_index, reinterpret_cast(output), stream); + break; + case nvinfer1::DataType::kINT32: + gather_topk_impl( + reinterpret_cast(data), reinterpret_cast(indices), dims, nbDims, + dims_indices, nbDims_index, reinterpret_cast(output), stream); + break; + default: + break; + } + return 0; +} + +// PluginCreator +GatherTopkCreator::GatherTopkCreator() +{ + mPluginAttributes.clear(); + mFC.nbFields = mPluginAttributes.size(); + mFC.fields = mPluginAttributes.data(); +} + +const char * GatherTopkCreator::getPluginName() const noexcept +{ + return PLUGIN_NAME; +} + +const char * GatherTopkCreator::getPluginVersion() const noexcept +{ + return PLUGIN_VERSION; +} + +const nvinfer1::PluginFieldCollection * GatherTopkCreator::getFieldNames() noexcept +{ + return &mFC; +} + +void GatherTopkCreator::setPluginNamespace(const char * pluginNamespace) noexcept +{ + mNamespace = pluginNamespace; +} + +const char * GatherTopkCreator::getPluginNamespace() const noexcept +{ + return mNamespace.c_str(); +} + +nvinfer1::IPluginV2 * GatherTopkCreator::createPlugin( + const char * name, const nvinfer1::PluginFieldCollection *) noexcept +{ + auto * plugin = new GatherTopk(name); + plugin->setPluginNamespace(getPluginNamespace()); + return plugin; +} + +nvinfer1::IPluginV2 * GatherTopkCreator::deserializePlugin( + const char * name, const void * serialData, size_t serialLength) noexcept +{ + auto plugin = new GatherTopk(name, serialData, serialLength); + plugin->setPluginNamespace(getPluginNamespace()); + return plugin; +} + +} // namespace ssd diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/plugins/gather_topk_kernel.cu b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/gather_topk_kernel.cu new file mode 100644 index 0000000000000..bcaef062d4af7 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/gather_topk_kernel.cu @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#include "cuda_utils.hpp" +#include "gather_topk_kernel.hpp" +#include "trt_plugin_helper.hpp" + +#include +#include +#include + +namespace ssd +{ +template +__global__ void gather_topk_kernel( + const scalar_t * input, const int * indices, scalar_t * output, int batch, int num_input, + int num_indices, int channel) +{ + CUDA_1D_KERNEL_LOOP(index, batch * num_indices * channel) + { + const int b_id = index / (num_indices * channel); + const int n_id = (index / channel) % num_indices; + const int c_id = index % channel; + + const int input_n_id = indices[b_id * num_indices + n_id]; + const scalar_t value = input[b_id * num_input * channel + input_n_id * channel + c_id]; + output[b_id * num_indices * channel + n_id * channel + c_id] = value; + } +} + +template +void gather_topk_impl( + const scalar_t * input, const int * indices, const int * dims, int nbDims, + const int * dims_indices, int nbDims_index, scalar_t * output, cudaStream_t stream) +{ + int batch = 1; + for (int i = 0; i < nbDims_index - 1; ++i) { + batch *= dims[i]; + } + int num_input = dims[nbDims_index - 1]; + int num_indices = dims_indices[nbDims_index - 1]; + int channel = 1; + for (int i = nbDims_index; i < nbDims; ++i) { + channel *= dims[i]; + } + const int col_block = DIVUP(batch * num_indices * channel, THREADS_PER_BLOCK); + gather_topk_kernel<<>>( + input, indices, output, batch, num_input, num_indices, channel); +} + +template void gather_topk_impl( + const float * input, const int * indices, const int * dims, int nbDims, const int * indices_dims, + int indices_nbDims, float * output, cudaStream_t stream); + +template void gather_topk_impl( + const int32_t * input, const int * indices, const int * dims, int nbDims, + const int * indices_dims, int indices_nbDims, int32_t * output, cudaStream_t stream); +} // namespace ssd diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/plugins/grid_priors.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/grid_priors.cpp new file mode 100644 index 0000000000000..3b3ab2d59aaa5 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/grid_priors.cpp @@ -0,0 +1,240 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#include "grid_priors.hpp" + +#include "grid_priors_kernel.hpp" +#include "trt_serialize.hpp" + +#include + +namespace ssd +{ +namespace +{ +static const char * PLUGIN_VERSION{"1"}; +static const char * PLUGIN_NAME{"GridPriorsTRT"}; +} // namespace + +GridPriors::GridPriors(const std::string & name, const nvinfer1::Dims & stride) +: mLayerName(name), mStride(stride) +{ +} + +GridPriors::GridPriors(const std::string & name, const void * data, size_t length) +: mLayerName(name) +{ + deserialize_value(&data, &length, &mStride); +} + +// IPluginV2 methods +const char * GridPriors::getPluginVersion() const noexcept +{ + return PLUGIN_VERSION; +} + +const char * GridPriors::getPluginType() const noexcept +{ + return PLUGIN_NAME; +} + +int GridPriors::initialize() noexcept +{ + return STATUS_SUCCESS; +} + +void GridPriors::terminate() noexcept +{ +} + +void GridPriors::destroy() noexcept +{ + delete this; +} + +void GridPriors::setPluginNamespace(const char * pluginNamespace) noexcept +{ + mNamespace = pluginNamespace; +} + +const char * GridPriors::getPluginNamespace() const noexcept +{ + return mNamespace.c_str(); +} + +int GridPriors::getNbOutputs() const noexcept +{ + return 1; +} + +size_t GridPriors::getSerializationSize() const noexcept +{ + return serialized_size(mStride); +} + +void GridPriors::serialize(void * buffer) const noexcept +{ + serialize_value(&buffer, mStride); +} + +// IPluginV2Ext methods +nvinfer1::DataType GridPriors::getOutputDataType( + int, const nvinfer1::DataType * inputTypes, int) const noexcept +{ + return inputTypes[0]; +} + +// IPluginV2DynamicExt methods +nvinfer1::IPluginV2DynamicExt * GridPriors::clone() const noexcept +{ + GridPriors * plugin = new GridPriors(mLayerName, mStride); + plugin->setPluginNamespace(getPluginNamespace()); + return plugin; +} + +nvinfer1::DimsExprs GridPriors::getOutputDimensions( + int, const nvinfer1::DimsExprs * inputs, int, nvinfer1::IExprBuilder & exprBuilder) noexcept +{ + nvinfer1::DimsExprs ret; + ret.nbDims = 2; + auto area = + exprBuilder.operation(nvinfer1::DimensionOperation::kPROD, *inputs[2].d[0], *inputs[1].d[0]); + ret.d[0] = exprBuilder.operation(nvinfer1::DimensionOperation::kPROD, *area, *(inputs[0].d[0])); + ret.d[1] = exprBuilder.constant(4); + + return ret; +} + +bool GridPriors::supportsFormatCombination( + int pos, const nvinfer1::PluginTensorDesc * ioDesc, int nbInputs, int) noexcept +{ + if (pos == 0) { + return ( + ioDesc[pos].type == nvinfer1::DataType::kFLOAT && + ioDesc[pos].format == nvinfer1::TensorFormat::kLINEAR); + } else if (pos == nbInputs) { + return ioDesc[pos].type == ioDesc[0].type && ioDesc[pos].format == ioDesc[0].format; + } else { + return true; + } +} + +void GridPriors::configurePlugin( + const nvinfer1::DynamicPluginTensorDesc *, int, const nvinfer1::DynamicPluginTensorDesc *, + int) noexcept +{ +} + +size_t GridPriors::getWorkspaceSize( + const nvinfer1::PluginTensorDesc *, int, const nvinfer1::PluginTensorDesc *, int) const noexcept +{ + return 0; +} + +int GridPriors::enqueue( + const nvinfer1::PluginTensorDesc * inputDesc, const nvinfer1::PluginTensorDesc *, + const void * const * inputs, void * const * outputs, void *, cudaStream_t stream) noexcept +{ + int num_base_anchors = inputDesc[0].dims.d[0]; + int feat_h = inputDesc[1].dims.d[0]; + int feat_w = inputDesc[2].dims.d[0]; + + const void * base_anchor = inputs[0]; + void * output = outputs[0]; + + auto data_type = inputDesc[0].type; + switch (data_type) { + case nvinfer1::DataType::kFLOAT: + grid_priors_impl( + reinterpret_cast(base_anchor), reinterpret_cast(output), + num_base_anchors, feat_w, feat_h, mStride.d[0], mStride.d[1], stream); + break; + default: + return 1; + } + return 0; +} + +// PluginCreator +GridPriorsCreator::GridPriorsCreator() +{ + mPluginAttributes.clear(); + mPluginAttributes.emplace_back(nvinfer1::PluginField("stride_h")); + mPluginAttributes.emplace_back(nvinfer1::PluginField("stride_w")); + mFC.nbFields = mPluginAttributes.size(); + mFC.fields = mPluginAttributes.data(); +} + +const char * GridPriorsCreator::getPluginName() const noexcept +{ + return PLUGIN_NAME; +} + +const char * GridPriorsCreator::getPluginVersion() const noexcept +{ + return PLUGIN_VERSION; +} + +const nvinfer1::PluginFieldCollection * GridPriorsCreator::getFieldNames() noexcept +{ + return &mFC; +} + +void GridPriorsCreator::setPluginNamespace(const char * pluginNamespace) noexcept +{ + mNamespace = pluginNamespace; +} + +const char * GridPriorsCreator::getPluginNamespace() const noexcept +{ + return mNamespace.c_str(); +} + +nvinfer1::IPluginV2 * GridPriorsCreator::createPlugin( + const char * name, const nvinfer1::PluginFieldCollection * fc) noexcept +{ + int stride_h = 1; + int stride_w = 1; + + for (int i = 0; i < fc->nbFields; ++i) { + if (fc->fields[i].data == nullptr) { + continue; + } + std::string field_name(fc->fields[i].name); + + if (field_name.compare("stride_w") == 0) { + stride_w = static_cast(fc->fields[i].data)[0]; + } + if (field_name.compare("stride_h") == 0) { + stride_h = static_cast(fc->fields[i].data)[0]; + } + } + nvinfer1::Dims stride{2, {stride_w, stride_h}}; + + GridPriors * plugin = new GridPriors(name, stride); + plugin->setPluginNamespace(getPluginNamespace()); + return plugin; +} + +nvinfer1::IPluginV2 * GridPriorsCreator::deserializePlugin( + const char * name, const void * serialData, size_t serialLength) noexcept +{ + auto plugin = new GridPriors(name, serialData, serialLength); + plugin->setPluginNamespace(getPluginNamespace()); + return plugin; +} + +} // namespace ssd diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/plugins/grid_priors_kernel.cu b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/grid_priors_kernel.cu new file mode 100644 index 0000000000000..70bb819a83b63 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/src/plugins/grid_priors_kernel.cu @@ -0,0 +1,61 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) OpenMMLab. All rights reserved. + +#include "cuda_utils.hpp" +#include "grid_priors_kernel.hpp" +#include "trt_plugin_helper.hpp" + +#include + +template +__global__ void grid_priors_kernel( + const scalar_t * base_anchor, scalar_t * output, int num_base_anchors, int feat_w, int feat_h, + int stride_w, int stride_h) +{ + extern __shared__ scalar_t shared_base_anchor[]; + for (int i = threadIdx.x; i < num_base_anchors * 4; i += blockDim.x) { + shared_base_anchor[i] = base_anchor[i]; + } + __syncthreads(); + + CUDA_1D_KERNEL_LOOP(index, num_base_anchors * feat_w * feat_h) + { + const int a_offset = (index % num_base_anchors) << 2; + const scalar_t w = static_cast(((index / num_base_anchors) % feat_w) * stride_w); + const scalar_t h = static_cast((index / (feat_w * num_base_anchors)) * stride_h); + + auto out_start = output + index * 4; + out_start[0] = shared_base_anchor[a_offset] + w; + out_start[1] = shared_base_anchor[a_offset + 1] + h; + out_start[2] = shared_base_anchor[a_offset + 2] + w; + out_start[3] = shared_base_anchor[a_offset + 3] + h; + } +} + +template +void grid_priors_impl( + const scalar_t * base_anchor, scalar_t * output, int num_base_anchors, int feat_w, int feat_h, + int stride_w, int stride_h, cudaStream_t stream) +{ + grid_priors_kernel<<< + GET_BLOCKS(num_base_anchors * feat_w * feat_h), THREADS_PER_BLOCK, + DIVUP(num_base_anchors * 4, 32) * 32 * sizeof(scalar_t), stream>>>( + base_anchor, output, num_base_anchors, feat_w, feat_h, stride_w, stride_h); +} + +template void grid_priors_impl( + const float * base_anchor, float * output, int num_base_anchors, int feat_w, int feat_h, + int stride_w, int stride_h, cudaStream_t stream); diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index 9b7bf632e1019..22aa0f6bf3880 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "trt_ssd.hpp" + +#include "gather_topk.hpp" +#include "grid_priors.hpp" #include #include @@ -169,33 +172,11 @@ void Net::infer(std::vector & buffers, const int batch_size) if (!context_) { throw std::runtime_error("Fail to create context"); } - auto input_dims = engine_->getBindingDimensions(0); + auto input_dims = getTensorShape("input"); context_->setBindingDimensions( 0, nvinfer1::Dims4(batch_size, input_dims.d[1], input_dims.d[2], input_dims.d[3])); context_->enqueueV2(buffers.data(), stream_, nullptr); cudaStreamSynchronize(stream_); } -std::vector Net::getInputSize() -{ - auto dims = engine_->getBindingDimensions(0); - return {dims.d[1], dims.d[2], dims.d[3]}; -} - -std::vector Net::getOutputScoreSize() -{ - auto dims = engine_->getBindingDimensions(1); - return {dims.d[1], dims.d[2]}; -} - -int Net::getMaxBatchSize() -{ - return engine_->getProfileDimensions(0, 0, nvinfer1::OptProfileSelector::kMAX).d[0]; -} - -int Net::getMaxDetections() -{ - return engine_->getBindingDimensions(1).d[1]; -} - } // namespace ssd diff --git a/perception/traffic_light_ssd_fine_detector/package.xml b/perception/traffic_light_ssd_fine_detector/package.xml index 95ce29227b6ff..49f67d171295a 100644 --- a/perception/traffic_light_ssd_fine_detector/package.xml +++ b/perception/traffic_light_ssd_fine_detector/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs cv_bridge diff --git a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp index 17661c44863bd..96014800ce842 100644 --- a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -48,6 +48,13 @@ TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet( const std::string onnx_file = this->declare_parameter("onnx_file"); const std::string label_file = this->declare_parameter("label_file"); const std::string mode = this->declare_parameter("mode", "FP32"); + dnn_header_type_ = this->declare_parameter("dnn_header_type", "pytorch"); + + if (dnn_header_type_.compare("pytorch") != 0 && dnn_header_type_.compare("mmdetection") != 0) { + RCLCPP_ERROR( + this->get_logger(), "Unexpected DNN type: %s, choose from (pytorch, mmdetection)", + dnn_header_type_.c_str()); + } fs::path engine_path{onnx_file}; engine_path.replace_extension("engine"); @@ -78,11 +85,12 @@ TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet( net_ptr_.reset(new ssd::Net(onnx_file, mode, max_batch_size)); net_ptr_->save(engine_path); } - channel_ = net_ptr_->getInputSize()[0]; - width_ = net_ptr_->getInputSize()[1]; - height_ = net_ptr_->getInputSize()[2]; - detection_per_class_ = net_ptr_->getOutputScoreSize()[0]; - class_num_ = net_ptr_->getOutputScoreSize()[1]; + + input_shape_ = net_ptr_->getInputShape(); + box_dims_ = net_ptr_->getOutputDimensions("boxes"); + score_dims_ = net_ptr_->getOutputDimensions("scores"); + detection_per_class_ = score_dims_->d[0]; + class_num_ = score_dims_->d[1]; is_approximate_sync_ = this->declare_parameter("approximate_sync", false); score_thresh_ = this->declare_parameter("score_thresh", 0.7); @@ -146,10 +154,15 @@ void TrafficLightSSDFineDetectorNodelet::callback( const int batch_size = net_ptr_->getMaxBatchSize(); while (num_rois != 0) { const int num_infer = (num_rois / batch_size > 0) ? batch_size : num_rois % batch_size; - auto data_d = cuda::make_unique(num_infer * channel_ * width_ * height_); - auto scores_d = cuda::make_unique(num_infer * detection_per_class_ * class_num_); - auto boxes_d = cuda::make_unique(num_infer * detection_per_class_ * 4); - std::vector buffers = {data_d.get(), scores_d.get(), boxes_d.get()}; + auto data_d = cuda::make_unique(num_infer * input_shape_.size()); + auto box_d = cuda::make_unique(num_infer * box_dims_->size()); + auto score_d = cuda::make_unique(num_infer * score_dims_->size()); + std::vector buffers; + if (dnn_header_type_.compare("pytorch") == 0) { + buffers = {data_d.get(), score_d.get(), box_d.get()}; + } else { + buffers = {data_d.get(), box_d.get(), score_d.get()}; + } std::vector lts, rbs; std::vector cropped_imgs; @@ -164,7 +177,7 @@ void TrafficLightSSDFineDetectorNodelet::callback( cropped_imgs.push_back(cv::Mat(original_image, cv::Rect(lts.at(i), rbs.at(i)))); } - std::vector data(num_infer * channel_ * width_ * height_); + std::vector data(num_infer * input_shape_.size()); if (!cvMat2CnnInput(cropped_imgs, num_infer, data)) { RCLCPP_ERROR(this->get_logger(), "Fail to preprocess image"); return; @@ -182,10 +195,10 @@ void TrafficLightSSDFineDetectorNodelet::callback( auto scores = std::make_unique(num_infer * detection_per_class_ * class_num_); auto boxes = std::make_unique(num_infer * detection_per_class_ * 4); cudaMemcpy( - scores.get(), scores_d.get(), sizeof(float) * num_infer * detection_per_class_ * class_num_, + boxes.get(), box_d.get(), sizeof(float) * num_infer * box_dims_->size(), cudaMemcpyDeviceToHost); cudaMemcpy( - boxes.get(), boxes_d.get(), sizeof(float) * num_infer * detection_per_class_ * 4, + scores.get(), score_d.get(), sizeof(float) * num_infer * score_dims_->size(), cudaMemcpyDeviceToHost); // Get Output std::vector detections; @@ -230,7 +243,7 @@ bool TrafficLightSSDFineDetectorNodelet::cvMat2CnnInput( // cv::Mat rgb; // cv::cvtColor(in_imgs.at(i), rgb, CV_BGR2RGB); cv::Mat resized; - cv::resize(in_imgs.at(i), resized, cv::Size(width_, height_)); + cv::resize(in_imgs.at(i), resized, cv::Size(input_shape_.width, input_shape_.height)); cv::Mat pixels; resized.convertTo(pixels, CV_32FC3, 1.0 / 255, 0); @@ -243,10 +256,10 @@ bool TrafficLightSSDFineDetectorNodelet::cvMat2CnnInput( return false; } - for (int c = 0; c < channel_; ++c) { - for (int j = 0, hw = width_ * height_; j < hw; ++j) { - data[i * channel_ * width_ * height_ + c * hw + j] = - (img[channel_ * j + 2 - c] - mean_[c]) / std_[c]; + for (int c = 0; c < input_shape_.channel; ++c) { + for (int j = 0, hw = input_shape_.area(); j < hw; ++j) { + data[i * input_shape_.size() + c * hw + j] = + (img[input_shape_.channel * j + 2 - c] - mean_[c]) / std_[c]; } } } @@ -270,10 +283,19 @@ bool TrafficLightSSDFineDetectorNodelet::cnnOutput2BoxDetection( size_t index = std::distance(tlr_scores.begin(), iter); size_t box_index = i * detection_per_class_ * 4 + index * 4; cv::Point lt, rb; - lt.x = boxes[box_index] * in_imgs.at(i).cols; - lt.y = boxes[box_index + 1] * in_imgs.at(i).rows; - rb.x = boxes[box_index + 2] * in_imgs.at(i).cols; - rb.y = boxes[box_index + 3] * in_imgs.at(i).rows; + if (dnn_header_type_.compare("pytorch") == 0) { + lt.x = boxes[box_index] * in_imgs.at(i).cols; + lt.y = boxes[box_index + 1] * in_imgs.at(i).rows; + rb.x = boxes[box_index + 2] * in_imgs.at(i).cols; + rb.y = boxes[box_index + 3] * in_imgs.at(i).rows; + } else { + const float dx = static_cast(in_imgs.at(i).cols) / input_shape_.width; + const float dy = static_cast(in_imgs.at(i).rows) / input_shape_.height; + lt.x = boxes[box_index] * dx; + lt.y = boxes[box_index + 1] * dy; + rb.x = boxes[box_index + 2] * dx; + rb.y = boxes[box_index + 3] * dy; + } fitInFrame(lt, rb, cv::Size(in_imgs.at(i).cols, in_imgs.at(i).rows)); det.x = lt.x; det.y = lt.y; diff --git a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp index e083dfe549a03..dfffde9018108 100644 --- a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp @@ -23,7 +23,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include #include diff --git a/perception/traffic_light_visualization/package.xml b/perception/traffic_light_visualization/package.xml index f16ddba519894..d90cf992b96be 100644 --- a/perception/traffic_light_visualization/package.xml +++ b/perception/traffic_light_visualization/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_perception_msgs cv_bridge diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index e4313f4eb8703..81fb08bbb59de 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -14,22 +14,27 @@ set(common_src src/behavior_path_planner_node.cpp src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/avoidance_by_lc/module.cpp + src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/pull_out/pull_out_module.cpp - src/scene_module/pull_over/pull_over_module.cpp + src/scene_module/goal_planner/goal_planner_module.cpp src/scene_module/side_shift/side_shift_module.cpp + src/scene_module/lane_change/interface.cpp + src/scene_module/lane_change/normal.cpp + src/scene_module/lane_change/external_request.cpp + src/scene_module/lane_change/avoidance_by_lane_change.cpp src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp src/utils/safety_check.cpp - src/utils/avoidance/util.cpp + src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp - src/utils/pull_over/util.cpp - src/utils/pull_over/shift_pull_over.cpp - src/utils/pull_over/geometric_pull_over.cpp - src/utils/pull_over/freespace_pull_over.cpp - src/utils/pull_over/goal_searcher.cpp + src/utils/goal_planner/util.cpp + src/utils/goal_planner/shift_pull_over.cpp + src/utils/goal_planner/geometric_pull_over.cpp + src/utils/goal_planner/freespace_pull_over.cpp + src/utils/goal_planner/goal_searcher.cpp + src/utils/goal_planner/default_fixed_goal_planner.cpp src/utils/pull_out/util.cpp src/utils/pull_out/shift_pull_out.cpp src/utils/pull_out/geometric_pull_out.cpp @@ -50,9 +55,6 @@ if(COMPILE_WITH_OLD_ARCHITECTURE) src/behavior_tree_manager.cpp src/scene_module/scene_module_bt_node_interface.cpp src/scene_module/lane_following/lane_following_module.cpp - src/scene_module/pull_over/pull_over_module.cpp - src/scene_module/lane_change/lane_change_module.cpp - src/scene_module/lane_change/external_request_lane_change_module.cpp ${common_src} ) @@ -64,14 +66,11 @@ else() ament_auto_add_library(behavior_path_planner_node SHARED src/planner_manager.cpp src/scene_module/avoidance/manager.cpp - src/scene_module/avoidance_by_lc/manager.cpp + src/scene_module/dynamic_avoidance/manager.cpp src/scene_module/pull_out/manager.cpp - src/scene_module/pull_over/manager.cpp + src/scene_module/goal_planner/manager.cpp src/scene_module/side_shift/manager.cpp src/scene_module/lane_change/manager.cpp - src/scene_module/lane_change/interface.cpp - src/scene_module/lane_change/normal.cpp - src/scene_module/lane_change/external_request.cpp ${common_src} ) @@ -109,6 +108,22 @@ if(BUILD_TESTING) behavior_path_planner_node ) + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module + behavior_path_planner_node + ) + + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_safety_check + test/test_safety_check.cpp + ) + + target_link_libraries(test_${CMAKE_PROJECT_NAME}_safety_check + behavior_path_planner_node + ) + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_turn_signal test/test_turn_signal.cpp ) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index fb4a96d4b9f44..47ccc9e58674a 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -16,16 +16,16 @@ The `behavior_path_planner` module is responsible to generate 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) | -| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | LINK | -| 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 | -| Pull Over | 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_pull_over_design.md) | -| Pull Out | 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_pull_out_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](./docs/behavior_path_planner_avoidance_design.md) | +| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | LINK | +| 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 | +| Goal 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) | +| Pull Out | 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_pull_out_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) | ![behavior_modules](./image/behavior_modules.png) @@ -43,10 +43,10 @@ The role of manager is to launch the appropriate scene module according to the s Now, it is able to select two managers with different architecture. -| Name | Description | Details | -| :------------------------------------ | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------- | -| Behavior Tree based manager (default) | this manager launches scene modules based on Behavior Tree. all scene modules run exclusively. | LINK(WIP) | -| BT-free manager (unstable) | this manager is developed in order to achieve complex scenario, and launches scene modules without Behavior Tree. multiple modules can run simultaneously on this manager. | LINK(WIP) | +| Name | Description | Details | +| :------------------------------------ | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------- | +| Behavior Tree based manager (default) | this manager launches scene modules based on Behavior Tree. all scene modules run exclusively. | LINK(WIP) | +| BT-free manager (unstable) | this manager is developed in order to achieve complex scenario, and launches scene modules without Behavior Tree. multiple modules can run simultaneously on this manager. | [LINK](./docs/behavior_path_planner_manager_design.md) | The manager is switched by flag `COMPILE_WITH_OLD_ARCHITECTURE` in CmakeLists.txt of `behavior_path_planner` package. Please set the flag **FALSE** if you try to use BT-free manager. @@ -91,13 +91,13 @@ The current behavior tree structure is shown below. Each modules (LaneChange, Av ### input -| Name | Type | Description | -| :----------------------------- | :----------------------------------------------------- | :--------------------------------------------------------------------------------- | -| ~/input/route | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/vector_map | `autoware_auto_mapping_msgs::msg::HADMapBin` | map information. | -| ~/input/objects | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Pull Over module. | -| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | +| Name | Type | Description | +| :----------------------------- | :----------------------------------------------------- | :------------------------------------------------------------------------------------ | +| ~/input/route | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/vector_map | `autoware_auto_mapping_msgs::msg::HADMapBin` | map information. | +| ~/input/objects | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | ## General features of behavior path planner diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 0183c227bce9c..e82aab23f9ca1 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -18,6 +18,7 @@ enable_safety_check: true enable_yield_maneuver: true disable_path_update: false + use_hatched_road_markings: false # for debug publish_debug_marker: false @@ -73,7 +74,9 @@ threshold_time_object_is_moving: 1.0 # [s] threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] # detection range - object_check_force_avoidance_clearance: 30.0 # [m] + object_ignore_distance_traffic_light: 30.0 # [m] + object_ignore_distance_crosswalk_forward: 30.0 # [m] + object_ignore_distance_crosswalk_backward: 30.0 # [m] object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] @@ -97,9 +100,8 @@ # avoidance lateral parameters lateral: lateral_collision_margin: 1.0 # [m] - lateral_passable_safety_buffer: 1.0 # [m] + lateral_execution_threshold: 0.499 # [m] road_shoulder_safety_margin: 0.3 # [m] - avoidance_execution_lateral_threshold: 0.499 max_right_shift_length: 5.0 max_left_shift_length: 5.0 # avoidance distance parameters @@ -143,3 +145,11 @@ matrix: [2.78, 13.9, # velocity [m/s] 0.50, 1.00] # margin [m] + + shift_line_pipeline: + trim: + quantize_filter_threshold: 0.2 + same_grad_filter_1_threshold: 0.1 + same_grad_filter_2_threshold: 0.2 + same_grad_filter_3_threshold: 0.5 + sharp_shift_filter_threshold: 0.2 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 7132f4057f381..d092feb2fc3de 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -40,3 +40,12 @@ rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 + + lane_following: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + + closest_lanelet: + distance_threshold: 5.0 # [m] + yaw_threshold: 0.79 # [rad] diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml index 017f052ba2d19..2538633e63bf0 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml @@ -4,8 +4,8 @@ - - + + @@ -61,10 +61,10 @@ - + desc - + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml index 5c4d2b589d242..65c600d246c59 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml @@ -4,8 +4,8 @@ - - + + @@ -66,10 +66,10 @@ - + desc - + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml index 0a615e48a140c..46b2492cca9fb 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml @@ -4,9 +4,9 @@ - - - + + + @@ -78,11 +78,11 @@ - + desc - - + + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml index bfae48368da48..c39ef30770b16 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml @@ -4,9 +4,9 @@ - - - + + + @@ -88,11 +88,11 @@ - + desc - - + + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml index e5876e9287594..70d6b37cb61e0 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml @@ -4,9 +4,9 @@ - - - + + + @@ -78,11 +78,11 @@ - + desc - - + + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml index e848f156a4e1a..80fe7ea8dac49 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml @@ -4,8 +4,8 @@ - - + + @@ -61,10 +61,10 @@ - + desc - + desc 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 902a93ce6d540..1609cdbc60b7a 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -1,30 +1,9 @@ /**: ros__parameters: # Static expansion - avoidance: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - lane_change: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - lane_following: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - pull_out: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - pull_over: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - side_shift: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] # Dynamic expansion by projecting the ego footprint along the path dynamic_expansion: 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 new file mode 100644 index 0000000000000..a4ff684839321 --- /dev/null +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -0,0 +1,32 @@ +/**: + ros__parameters: + dynamic_avoidance: + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: true + pedestrian: false + + min_obstacle_vel: 1.0 # [m/s] + + drivable_area_generation: + lat_offset_from_obstacle: 1.3 # [m] + max_lat_offset_to_avoid: 0.5 # [m] + + # for same directional object + overtaking_object: + max_time_to_collision: 3.0 # [s] + start_duration_to_avoid: 4.0 # [s] + end_duration_to_avoid: 5.0 # [s] + duration_to_hold_avoidance: 3.0 # [s] + + # for opposite directional object + oncoming_object: + max_time_to_collision: 3.0 # [s] + start_duration_to_avoid: 9.0 # [s] + end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml new file mode 100644 index 0000000000000..3c5846edf3ee9 --- /dev/null +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -0,0 +1,108 @@ +/**: + ros__parameters: + goal_planner: + # general params + minimum_request_length: 100.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. + + # goal search + goal_search: + search_priority: "efficient_path" # "efficient_path" or "close_goal" + parking_policy: "left_side" # "left_side" or "right_side" + forward_goal_search_length: 20.0 + backward_goal_search_length: 20.0 + goal_search_interval: 2.0 + longitudinal_margin: 3.0 + max_lateral_offset: 0.5 + lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.0 + margin_from_boundary: 0.5 + + # occupancy grid map + occupancy_grid: + use_occupancy_grid: true + use_occupancy_grid_for_longitudinal_margin: false + occupancy_grid_collision_check_margin: 0.0 + theta_size: 360 + obstacle_threshold: 60 + + # object recognition + object_recognition: + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 + + # pull over + pull_over: + pull_over_velocity: 3.0 + pull_over_minimum_velocity: 1.38 + decide_path_distance: 10.0 + maximum_deceleration: 1.0 + maximum_jerk: 1.0 + + # shift parking + shift_parking: + enable_shift_parking: true + shift_sampling_num: 4 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + after_shift_straight_distance: 1.0 + + # parallel parking path + parallel_parking: + path_interval: 1.0 + max_steer_angle: 0.35 # 20deg + forward: + enable_arc_forward_parking: true + after_forward_parking_straight_distance: 2.0 + forward_parking_velocity: 1.38 + forward_parking_lane_departure_margin: 0.0 + forward_parking_path_interval: 1.0 + forward_parking_max_steer_angle: 0.35 # 20deg + backward: + enable_arc_backward_parking: true + after_backward_parking_straight_distance: 2.0 + backward_parking_velocity: -1.38 + backward_parking_lane_departure_margin: 0.0 + backward_parking_path_interval: 1.0 + backward_parking_max_steer_angle: 0.35 # 20deg + + # freespace parking + freespace_parking: + enable_freespace_parking: true + freespace_parking_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 + + # debug + debug: + print_debug_info: false diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 87d6caa848b41..031ddc08fba1d 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -1,24 +1,25 @@ /**: ros__parameters: lane_change: - lane_change_prepare_duration: 4.0 # [s] - lane_changing_safety_check_duration: 8.0 # [s] + backward_lane_length: 200.0 #[m] prepare_duration: 4.0 # [s] - minimum_prepare_length: 2.0 # [m] - minimum_lane_changing_length: 16.5 # [m] backward_length_buffer_for_end_of_lane: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] - lane_changing_lateral_acc: 0.315 # [m/s2] - lane_changing_lateral_acc_at_low_velocity: 0.15 # [m/s2] lateral_acc_switching_velocity: 4.0 #[m/s] minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] - maximum_deceleration: 1.0 # [m/s2] - lane_change_sampling_num: 3 + longitudinal_acceleration_sampling_num: 5 + lateral_acceleration_sampling_num: 3 + + # lateral acceleration map + lateral_acceleration: + velocity: [0.0, 4.0, 10.0] + min_values: [0.15, 0.15, 0.15] + max_values: [0.5, 0.5, 0.5] # target object target_object: @@ -41,7 +42,8 @@ enable_cancel_lane_change: true enable_abort_lane_change: false - abort_delta_time: 3.0 # [s] + abort_delta_time: 1.0 # [s] + aborting_time: 5.0 # [s] abort_max_lateral_jerk: 1000.0 # [m/s3] # debug diff --git a/planning/behavior_path_planner/config/lane_following/lane_following.param.yaml b/planning/behavior_path_planner/config/lane_following/lane_following.param.yaml deleted file mode 100644 index 8263622f1d936..0000000000000 --- a/planning/behavior_path_planner/config/lane_following/lane_following.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - lane_following: - lane_change_prepare_duration: 2.0 - - closest_lanelet: - distance_threshold: 5.0 # [m] - yaw_threshold: 0.79 # [rad] diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml deleted file mode 100644 index 854cfd3cb2680..0000000000000 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ /dev/null @@ -1,97 +0,0 @@ -/**: - ros__parameters: - pull_over: - request_length: 100.0 - minimum_request_length: 200.0 - th_arrived_distance: 1.0 - th_stopped_velocity: 0.01 - th_stopped_time: 2.0 # It must be greater than the state_machine's. - pull_over_velocity: 3.0 - pull_over_minimum_velocity: 1.38 - margin_from_boundary: 0.5 - decide_path_distance: 10.0 - maximum_deceleration: 1.0 - maximum_jerk: 1.0 - # goal research - enable_goal_research: true - search_priority: "efficient_path" # "efficient_path" or "close_goal" - forward_goal_search_length: 20.0 - backward_goal_search_length: 20.0 - goal_search_interval: 2.0 - longitudinal_margin: 3.0 - max_lateral_offset: 0.5 - lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 - # occupancy grid map - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false - occupancy_grid_collision_check_margin: 0.0 - theta_size: 360 - obstacle_threshold: 60 - # object recognition - use_object_recognition: true - object_recognition_collision_check_margin: 1.0 - # shift path - enable_shift_parking: true - pull_over_sampling_num: 4 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - after_pull_over_straight_distance: 1.0 - # freespace parking - enable_freespace_parking: true - freespace_parking: - planning_algorithm: "astar" # options: astar, rrtstar - velocity: 1.0 - vehicle_shape_margin: 1.0 - time_limit: 3000.0 - minimum_turning_radius: 5.0 - maximum_turning_radius: 5.0 - turning_radius_size: 1 - # search configs - theta_size: 144 - angle_goal_range: 6.0 - curve_weight: 1.2 - reverse_weight: 1.0 - lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 - # costmap configs - obstacle_threshold: 30 - # -- A* search Configurations -- - astar: - only_behind_solutions: false - use_back: false - distance_heuristic_weight: 1.0 - # -- RRT* search Configurations -- - rrtstar: - enable_update: true - use_informed_sampling: true - max_planning_time: 150.0 - neighbor_radius: 8.0 - margin: 1.0 - # parallel parking path - enable_arc_forward_parking: true - enable_arc_backward_parking: true - after_forward_parking_straight_distance: 2.0 - after_backward_parking_straight_distance: 2.0 - forward_parking_velocity: 1.38 - backward_parking_velocity: -1.38 - forward_parking_lane_departure_margin: 0.0 - backward_parking_lane_departure_margin: 0.0 - arc_path_interval: 1.0 - pull_over_max_steer_angle: 0.35 # 20deg - # hazard on when parked - hazard_on_threshold_distance: 1.0 - hazard_on_threshold_velocity: 0.5 - # check safety with dynamic objects. Not used now. - pull_over_duration: 2.0 - pull_over_prepare_duration: 4.0 - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - prediction_time_resolution: 0.5 - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - # debug - print_debug_info: false diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 488fdce103216..30626a4df30c4 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -45,7 +45,7 @@ priority: 2 max_module_size: 1 - pull_over: + goal_planner: enable_module: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false @@ -65,3 +65,10 @@ enable_simultaneous_execution_as_candidate_module: false priority: 3 max_module_size: 1 + + dynamic_avoidance: + enable_module: false + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: true + priority: 7 + max_module_size: 1 diff --git a/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml b/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml index 815301d55aa52..62aac0b9b4804 100644 --- a/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml +++ b/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml @@ -7,3 +7,4 @@ min_shifting_distance: 5.0 min_shifting_speed: 5.56 shift_request_time_limit: 1.0 + publish_debug_marker: false 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 index 426d062ac68a2..e5c18a9ca35b4 100644 --- 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 @@ -57,7 +57,7 @@ This section gives details of the generation of the drivable area (`left_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`, `Pull Over`, `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. +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 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md new file mode 100644 index 0000000000000..f303937bd0f78 --- /dev/null +++ b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md @@ -0,0 +1,62 @@ +# Dynamic avoidance design + +## Purpose / Role + +This is a module designed for avoiding obstacles which are running. +Static obstacles such as parked vehicles are dealt with by the avoidance module. + +This module is under development. +In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area. +Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory. + +## Overview of drivable area modification + +### Filtering obstacles to avoid + +The dynamics obstacles meeting the following condition will be avoided. + +- The type is designated as `target_object.*`. +- The norm of the obstacle's velocity projected to the ego's path is smaller than `target_object.min_obstacle_vel`. +- The obstacle is in the next lane to the ego's lane, which will not cut-into the ego's lane according to the highest-prioritized predicted path. + +### Drivable area modification + +To realize dynamic obstacles for avoidance, the time dimension should be take into an account considering the dynamics. +However, it will make the planning problem much harder to solve. +Therefore, we project the time dimension to the 2D pose dimension. + +Currently, the predicted paths of predicted objects are not so stable. +Therefore, instead of using the predicted paths, we assume that the obstacle will run parallel to the ego's path. + +First, a maximum lateral offset to avoid is calculated as follows. +The polygon's width to extract from the drivable area is the obstacle width and double `drivable_area_generation.lat_offset_from_obstacle`. +We can limit the lateral shift offset by `drivable_area_generation.max_lat_offset_to_avoid`. + +![drivable_area_extraction_width](../image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg) + +Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision). +Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g. The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.). + +Same directional obstacles +![same_directional_object](../image/dynamic_avoidance/same_directional_object.svg) + +Opposite directional obstacles +![opposite_directional_object](../image/dynamic_avoidance/opposite_directional_object.svg) + +## Parameters + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------------------- | :---- | :----- | :--------------------------------------------------------- | :------------ | +| target_object.car | [-] | bool | The flag whether to avoid cars or not | true | +| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true | +| ... | [-] | bool | ... | ... | +| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 | +| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 | +| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 | +| drivable_area_generation.overtaking_object.max_time_to_collision | [s] | double | Maximum value when calculating time to collision | 3.0 | +| drivable_area_generation.overtaking_object.start_duration_to_avoid | [s] | double | Duration to consider avoidance before passing by obstacles | 4.0 | +| drivable_area_generation.overtaking_object.end_duration_to_avoid | [s] | double | Duration to consider avoidance after passing by obstacles | 5.0 | +| drivable_area_generation.overtaking_object.duration_to_hold_avoidance | [s] | double | Duration to hold avoidance after passing by obstacles | 3.0 | +| drivable_area_generation.oncoming_object.max_time_to_collision | [s] | double | Maximum value when calculating time to collision | 3.0 | +| drivable_area_generation.oncoming_object.start_duration_to_avoid | [s] | double | Duration to consider avoidance before passing by obstacles | 9.0 | +| drivable_area_generation.oncoming_object.end_duration_to_avoid | [s] | double | Duration to consider avoidance after passing by obstacles | 0.0 | diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md similarity index 75% rename from planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md rename to planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index a593bfb806a2a..4a0f6944be561 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -1,51 +1,55 @@ -# Pull Over design +# Goal Planner design ## Purpose / Role -Search for a space where there are no objects and pull over there. +Plan path around the goal. + +- Park at the designated goal. +- Modify the goal to avoid obstacles or to pull over at the side of tha lane. ## Design +If goal modification is not allowed, park at the designated fixed goal. (`fixed_goal_planner` in the figure below) +When allowed, park in accordance with the specified policy(e.g pull over on left/right side of the lane). (`rough_goal_planner` in the figure below). Currently rough goal planner only support pull_over feature, but it would be desirable to be able to accommodate various parking policies in the future. + ```plantuml @startuml -package pull_over{ - abstract class PullOverPlannerBase { - } - abstract class GoalSeacherBase { - } +package goal_planner{ - package lane_parking <>{ - class ShiftPullOver { - } - class GeometricPullOver { + class GoalPlannerModule {} + + package rough_goal_planner <>{ + + package lane_parking <>{ + class ShiftPullOver {} + class GeometricPullOver {} } - } - package freespace_parking <>{ - class FreeSpacePullOver { + package freespace_parking <>{ + class FreeSpacePullOver {} } - } - class GoalSeacher { - } + class GoalSeacher {} - class PullOverModule { - } + struct GoalCandidates {} + struct PullOverPath{} + abstract class PullOverPlannerBase {} + abstract class GoalSeacherBase {} - struct GoalCandidates { } - struct PullOverPath{} + package fixed_goal_planner <>{ + abstract class FixedGoalPlannerBase {} + class DefaultFixedPlanner{} + } } package utils{ - class PathShifter { - } + class PathShifter {} - class GeometricParallelParking { - } + class GeometricParallelParking {} } package freespace_planning_algorithms @@ -54,19 +58,21 @@ package freespace_planning_algorithms class RRTStar{} } -' pull over +' goal planner ShiftPullOver --|> PullOverPlannerBase GeometricPullOver --|> PullOverPlannerBase FreeSpacePullOver --|> PullOverPlannerBase GoalSeacher --|> GoalSeacherBase +DefaultFixedPlanner --|> FixedGoalPlannerBase PathShifter --o ShiftPullOver GeometricParallelParking --o GeometricPullOver AstarSearch --o FreeSpacePullOver RRTStar --o FreeSpacePullOver -PullOverPlannerBase --o PullOverModule -GoalSeacherBase --o PullOverModule +PullOverPlannerBase --o GoalPlannerModule +GoalSeacherBase --o GoalPlannerModule +FixedGoalPlannerBase --o GoalPlannerModule PullOverPath --o PullOverPlannerBase GoalCandidates --o GoalSeacherBase @@ -74,15 +80,51 @@ GoalCandidates --o GoalSeacherBase @enduml ``` -## General parameters for pull_over +## start condition + +Either one is activated when all conditions are met. + +### fixed_goal_planner + +- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- Route is set with `allow_goal_modification=false` by default. + + + +### rough_goal_planner + +#### pull over on road lane + +- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- Route is set with `allow_goal_modification=true` . + - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. + - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. +- ego-vehicle is in the same lane as the goal. + + + +#### pull over on shoulder lane + +- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- Goal is set in the `road_shoulder`. + + + +## finish condition + +- The distance to the goal from your vehicle is lower than threshold (default: < `1m`). +- The ego-vehicle is stopped. + - The speed is lower than threshold (default: < `0.01m/s`). + +## General parameters for goal_planner | Name | Unit | Type | Description | Default value | | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 200.0 | +| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | | th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | | th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | | th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 2.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | | pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | | margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | | decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | @@ -125,13 +167,12 @@ searched for in certain range of the shoulder lane. | Name | Unit | Type | Description | Default value | | :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | | search_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| enable_goal_research | [-] | double | flag whether to search goal | true | | forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | | backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | | goal_search_interval | [m] | double | distance interval for goal search | 2.0 | | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | | ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | ## **Path Generation** @@ -154,14 +195,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val #### Parameters for shift parking -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | -| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | -| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | -| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| shift_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_shift_straight_distance | [m] | double | straight line distance after pull over end point | 1.0 | ### **geometric parallel parking** @@ -212,7 +253,7 @@ 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) \*Series execution with `avoidance_module` in the flowchart is under development. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md new file mode 100644 index 0000000000000..4072e09f5df3a --- /dev/null +++ b/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md @@ -0,0 +1,5 @@ +# Interface design + +!!! warning + + Under Construction diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 03d2edeee53f2..3050b32d8e3a3 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -22,18 +22,18 @@ The lane change candidate path is divided into two phases: preparation and lane- The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows. ```C++ -lane_change_prepare_distance = max(current_speed * lane_change_prepare_duration + 0.5 * deceleration * lane_change_prepare_duration^2, minimum_lane_change_prepare_distance) +lane_change_prepare_distance = current_speed * lane_change_prepare_duration + 0.5 * deceleration * lane_change_prepare_duration^2 ``` During the preparation phase, the turn signal will be activated when the remaining distance is equal to or less than `lane_change_search_distance`. ### Lane-changing phase -The lane-changing phase consist of the shifted path that moves ego from current lane to the target lane. Total distance of lane-changing phase is as follows. +The lane-changing phase consist of the shifted path that moves ego from current lane to the target lane. Total distance of lane-changing phase is as follows. Note that during the lane changing phase, the ego vehicle travels at a constant speed. ```C++ -lane_change_prepare_velocity = current_speed + deceleration * lane_change_prepare_duration -lane_changing_distance = max(lane_change_prepare_velocity * lane_changing_duration + 0.5 * deceleration * lane_changing_duration^2, minimum_lane_change_length + backward_length_buffer_for_end_of_lane) +lane_change_prepare_velocity = std::max(current_speed + deceleration * lane_change_prepare_duration, minimum_lane_changing_velocity) +lane_changing_distance = lane_change_prepare_velocity * lane_changing_duration ``` The `backward_length_buffer_for_end_of_lane` is added to allow some window for any possible delay, such as control or mechanical delay during brake lag. @@ -41,14 +41,16 @@ The `backward_length_buffer_for_end_of_lane` is added to allow some window for a #### Multiple candidate path samples Lane change velocity is affected by the ego vehicle's current velocity. High velocity requires longer preparation and lane changing distance. However we also need to plan lane changing trajectories in case ego vehicle slows down. -Computing candidate paths that assumes ego vehicle's slows down is performed by substituting predetermined deceleration value into `lane_change_prepare_distance`, `lane_change_prepare_velocity` and `lane_changing_distance` equation. +Computing candidate paths that assumes ego vehicle's slows down is performed by substituting predetermined deceleration value into `prepare_length`, `prepare_velocity` and `lane_changing_length` equation. -The predetermined deceleration are a set of value that starts from `deceleration = 0.0`, and decrease by `acceleration_resolution` until it reaches`deceleration = -maximum_deceleration`. The `acceleration_resolution` is determine by the following +The predetermined deceleration are a set of value that starts from `deceleration = 0.0`, and decrease by `acceleration_resolution` until it reaches`deceleration = -maximum_deceleration`. `maximum_deceleration` is defined in the `common.param` file as `normal.min_acc`. The `acceleration_resolution` is determine by the following ```C++ acceleration_resolution = maximum_deceleration / lane_change_sampling_num ``` +Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, the acceleration becomes positive value (not deceleration). + The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. ![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) @@ -123,183 +125,7 @@ stop #### Candidate Path's Safety check -Valid candidate path is evaluated for safety before is was selected as the output candidate path. The flow of the process is as follows. - -```plantuml -@startuml -skinparam monochrome true -skinparam defaultTextAlignment center -skinparam noteTextAlignment left - -title Safe and Force Lane Change -start -:**INPUT** std::vector valid_paths; - -partition selectValidPaths { -:**INITIALIZE** std::vector valid_paths; - -:idx = 0; - -while (idx < input_paths.size()?) is (TRUE) - -:path = valid_paths.at(idx); - - -if(path pass safety check?) then (TRUE) -:selected_path = path, is_path_safe = true; -else (NO) -endif - -:++idx; -endwhile (FALSE) - -if(valid_paths.empty()?)then (true) - -:selected_path = valid_paths.front(), is_path_safe = false; -note left -used for -**FORCE LANE CHANGE** -if FORCE is needed, -then there is no safe path -end note -else (\nfalse) -endif - -:**RETURN** selected_path && is_path_safe; - -} -stop -@enduml - -``` - -If all valid candidate path is unsafe, then the operator will have the option to perform force lane change by using the front-most candidate path as the output. The force lane change will ignore all safety checks. - -A candidate path's is safe if it satisfies the following lateral distance criteria, - -```C++ -lateral distance > lateral_distance_threshold -``` - -However, suppose the lateral distance is insufficient. In that case, longitudinal distance will be evaluated. The candidate path is safe only when the longitudinal gap between the ego vehicle and the dynamic object is wide enough. - -The following charts illustrate the flow of the safety checks - -```plantuml -@startuml -skinparam monochrome true -skinparam defaultTextAlignment center -skinparam noteTextAlignment left - -title Collision/Safety Check -start -:**INPUT** valid_path; - -:**CONVERT** valid_path **to** ego_predicted_path; - -:idx = 0; - -:is_safe_path = true; - -:objects = dynamic_objects in current and target lane; -while (idx < objects.size()?) is (TRUE) - -:obj = objects.at(idx); - -:**INITIALIZE** time = 0; - -:check_time = prepare_duration + lane_changing_duration; - -while(time == check_time?) is (FALSE) - -if(lateral distance is sufficient) then (TRUE) - -else -if (is ego in front of object?) then (YES) -:v_front = v_ego -v_rear = v_obj; -else (NO) -:v_front = v_obj -v_rear = v_ego; -endif -if(longitudinal distance is sufficient) then (YES) -else (NO) -:is_safe_path = false; -break -endif -endif - -:t+=prediction_time_resolution; - -endwhile (TRUE) - -if(if_safe_path == TRUE?) then (YES) -else (NO) -break; -endif - -:++idx; - -endwhile (FALSE) - -:**RETURN** is_safe_path; - -stop -@enduml - -``` - -##### Calculating and evaluating longitudinal distance - -A sufficient longitudinal gap between vehicles will prevent any rear-end collision from happening. This includes an emergency stop or sudden braking scenarios. - -The following information is required to evaluate the longitudinal distance between vehicles - -1. estimated speed of the dynamic objects -2. predicted path of dynamic objects -3. ego vehicle's current speed -4. ego vehicle's predicted path (converted/estimated from candidate path) - -The following figure illustrates how the safety check is performed on ego vs. dynamic objects. - -![Safety check](../image/lane_change/lane_change-collision_check.png) - -Let `v_front` and `a_front` be the front vehicle's velocity and deceleration, respectively, and `v_rear` and `a_rear` be the rear vehicle's velocity and deceleration, respectively. -Front vehicle and rear vehicle assignment will depend on which predicted path's pose is currently being evaluated. - -The following figure illustrates front and rear vehicle velocity assignment. - -![front rear assignment](../image/lane_change/lane_change-collision_check_parked_vehicle.png) - -Assuming the front vehicle brakes, then `d_front` is the distance the front vehicle will travel until it comes to a complete stop. The distance is computed from the equation of motion, which yield. - -```C++ -d_front = -std::pow(v_front,2) / (2*a_front) -``` - -Using the same formula to evaluate the rear vehicle's stopping distance `d_rear` is insufficient. This is because as the rear vehicle's driver saw the front vehicle's sudden brake, it will take some time for the driver to process the information and react by pressing the brake. We call this delay the reaction time. - -The reaction time is considered from the duration starting from the driver seeing the front vehicle brake light until the brake is pressed. As the brake is pressed, the time margin (which might be caused by mechanical or control delay) also needs to be considered. With these two parameters included, the formula for `d_rear` will be as follows. - -```C++ -d_rear = v_rear * rear_vehicle_reaction_time + v_rear * rear_vehicle_safety_time_margin + (-std::pow(v_front,2) / 2 * a_rear) -``` - -Since there is no absolute value for the deceleration`a_front` and `a_rear`, both of the values are parameterized (`expected_front_deceleration` and `expected_rear_deceleration`, respectively) with the estimation of how much deceleration will occur if the brake is pressed. - -The longitudinal distance is evaluated as follows - -```C++ -d_rear < d_front + d_inter -``` - -where `d_inter` is the relative longitudinal distance obtained at each evaluated predicted pose. - -Finally minimum longitudinal distance for `d_rear` is added to compensate for object to near to each other when `d_rear = 0.0`. This yields - -```C++ -std::max(longitudinal_distance_min_threshold, d_rear) < d_front + d_inter -``` +See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md) ##### Collision check in prepare phase @@ -311,10 +137,11 @@ The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ig #### If the lane is blocked and multiple lane changes -When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance computation is as follows. +When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance can be computed from shift length and minimum lane changing velocity. ```C++ -minimum_lane_change_distance = num_of_lane_changes * (minimum_lane_change_length + backward_length_buffer_for_end_of_lane) +lane_changing_time = f(shift_length, lat_acceleration, lat_jerk) +minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + backward_length_buffer_for_end_of_lane ``` The following figure illustrates when the lane is blocked in multiple lane changes cases. @@ -407,20 +234,16 @@ The last behavior will also occur if the ego vehicle has departed from the curre The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | ------- | ------ | --------------------------------------------------------------------------------------- | ------------- | -| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | -| `minimum_lane_change_prepare_distance` | [m] | double | Minimum prepare distance for lane change | 2.0 | -| `minimum_lane_change_length` | [m] | double | The minimum distance needed for changing lanes. | 16.5 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `lane_changing_lateral_acc` | [m/s2] | double | Lateral acceleration value for lane change path generation | 0.5 | -| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | -| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | ------ | ------ | --------------------------------------------------------------------------------------- | ------------- | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | ### Collision checks during lane change diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md new file mode 100644 index 0000000000000..61ce21d514623 --- /dev/null +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -0,0 +1,605 @@ +# Manager design + +## Purpose / Role + +The manager launches and executes scene modules in `behavior_path_planner` depending on the use case, and has been developed to achieve following features: + +- Multiple modules can run simultaneously in series in order to achieve more complex use cases. For example, as shown in the following video, this manager make it possible to avoid a parked vehicle during lane change maneuver. +- Flexible development by not relying on framework from external libraries. + +![example](../image/manager/example_behavior.svg) + +[Movie](https://user-images.githubusercontent.com/44889564/231639251-6631dc4e-1861-47e7-9c51-6df1b666ce9b.mp4) + +Support status: + +| Name | Simple exclusive execution | Advanced simultaneous execution | +| :--------------------------------------- | :------------------------: | :-----------------------------: | +| Avoidance | :heavy_check_mark: | :heavy_check_mark: | +| Avoidance By Lane Change | :heavy_check_mark: | :heavy_multiplication_x: | +| Lane Change | :heavy_check_mark: | :heavy_check_mark: | +| External Lane Change | :heavy_check_mark: | :heavy_multiplication_x: | +| Goal Planner (without goal modification) | :heavy_check_mark: | :heavy_check_mark: | +| Goal Planner (with goal modification) | :heavy_check_mark: | :heavy_multiplication_x: | +| Pull Out | :heavy_check_mark: | :heavy_check_mark: | +| Side Shift | :heavy_check_mark: | :heavy_multiplication_x: | + +Click [here](../README.md) for supported scene modules. + +!!! warning + + It is still under development and some functions may be unstable. + +## Overview + +The manager is the core part of the `behavior_path_planner` implementation. It outputs path based on the latest data. + +The manager has sub-managers for each scene module, and its main task is + +- set latest planner data to scene modules via sub-managers. +- check scene module's request status via sub-managers. +- launch scene modules that make execution request. +- execute launched modules. +- delete scene expired modules. + +Additionally, the manager generates root reference path, and if any other modules don't request execution, the path is used as the planning result of `behavior_path_planner`. + +![manager_overview](../image/manager/manager_overview.svg) + +### Sub-managers + +The sub-manager's main task is + +- store the launched modules in internal vectors `registered_modules_`. +- create scene module instance. +- pass scene module's instance to the manager. +- delete expired scene module instance from `registered_modules_`. +- publish debug markers. + +
+ ![sub_managers](../image/manager/sub_managers.svg){width=1000} +
sub-managers
+
+ +Sub-manager is registered on the manager with the following function. + +```c++ +/** + * @brief register managers. + * @param manager pointer. + */ +void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) +{ + RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str()); + manager_ptrs_.push_back(manager_ptr); + processing_time_.emplace(manager_ptr->getModuleName(), 0.0); +} +``` + +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L66-L75) + +Sub-manager has the following parameters that are needed by the manager to manage the launched modules, and these parameters can be set for each module. + +```c++ +struct ModuleConfigParameters +{ + bool enable_module{false}; + bool enable_simultaneous_execution_as_approved_module{false}; + bool enable_simultaneous_execution_as_candidate_module{false}; + uint8_t priority{0}; + uint8_t max_module_size{0}; +}; +``` + +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp#L23-L30) + +| Name | Type | Description | +| :-------------------------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------- | +| `enable_module` | bool | if true, the sub-manager is registered on the manager. | +| `enable_simultaneous_execution_as_candidate_module` | bool | if true, the manager allows its scene modules to run with other scene modules as **candidate module**. | +| `enable_simultaneous_execution_as_approved_module` | bool | if true, the manager allows its scene modules to run with other scene modules as **approved module**. | +| `priority` | uint8_t | the manager decides execution priority based on this parameter. The smaller the number is, the higher the priority is. | +| `max_module_size` | uint8_t | the sub-manager can run some modules simultaneously. this parameter set the maximum number of the launched modules. | + +### Scene modules + +Scene modules receives necessary data and RTC command, and outputs candidate path(s), reference path and RTC cooperate status. When multiple modules run in series, the output of the previous module is received as input and the information is used to generate a new modified path, as shown in the following figure. And, when one module is running alone, it receives a reference path generated from the centerline of the lane in which Ego is currently driving as previous module output. + +
+ ![scene_module](../image/manager/scene_module.svg){width=1000} +
scene module
+
+ +| I/O | Type | Description | +| :-- | :-------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| IN | `behavior_path_planner::BehaviorModuleOutput` | previous module output. contains data necessary for path planning. | +| IN | `behavior_path_planner::PlannerData` | contains data necessary for path planning. | +| IN | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | +| OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc... | +| OUT | `tier4_planning_msgs::msg::CooperateStatus` | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | +| OUT | `autoware_auto_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | +| OUT | `autoware_auto_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | +| OUT | `visualization_msgs::msg::MarkerArray` | virtual wall, debug info, etc... | + +Scene modules running on the manager are stored on the **candidate modules stack** or **approved modules stack** depending on the condition whether the path modification has been approved or not. + +| Stack | Approval condition | Description | +| :---------------- | :----------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| candidate modules | Not approved | The candidate modules whose modified path has not been approved by [RTC](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md) is stored in vector `candidate_module_ptrs_` in the manager. The candidate modules stack is updated in the following order. 1. The manager selects only those modules that can be executed based on the configuration of the sub-manager whose scene module requests execution. 2. Determines the execution priority. 3. Executes them as candidate module. All of these modules receive the decided (approved) path from approved modules stack and **RUN in PARALLEL**.
![candidate_modules_stack](../image/manager/candidate_modules_stack.svg) | +| approved modules | Already approved | When the path modification is approved via RTC commands, the manager moves the candidate module to approved modules stack. These modules are stored in `approved_module_ptrs_`. In this stack, all scene modules **RUN in SERIES**.
![approved_modules_stack](../image/manager/approved_modules_stack.svg) | + +## Process flow + +There are 6 steps in one process: + +### Step1 + +At first, the manager set latest planner data, and run all approved modules and get output path. At this time, the manager checks module status and removes expired modules from approved modules stack. + +![process_step1](../image/manager/process_step1.svg) + +### Step2 + +Input approved modules output and necessary data to all registered modules, and the modules judge the necessity of path modification based on it. The manager checks which module makes execution request. + +![process_step2](../image/manager/process_step2.svg) + +### Step3 + +Check request module existence. + +### Step4 + +The manager decides which module to execute as candidate modules from the modules that requested to execute path modification. + +![process_step4](../image/manager/process_step4.svg) + +### Step5 + +Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status. + +![process_step5](../image/manager/process_step5.svg) + +### Step6 + +Move approved module to approved modules stack from candidate modules stack. + +![process_step6](../image/manager/process_step6.svg) + +--- + +and, within a single planning cycle, these steps are repeated until the following conditions are satisfied. + +- **Any modules don't make a request of path modification. (Check in Step3)** +- **Any candidate modules' request are not approved. (Check in Step5)** + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Manager processes + +start + +while () + +partition Step1 { +:generate reference path; +:set latest planner data; +:run all **approved modules** and get approved modules output; +:remove expired modules; +note left +remove modules whose status is +**ModuleStatus::SUCCESS** or +**ModuleStatus::FAILURE** +end note +} + +partition Step2 { +:set approved modules output to all registered scene modules; +:check which scene module makes a execution request; +} + +partition Step3 { +if(request modules exist) then (TRUE) +else (NO) +stop +note right +break from the loop +end note +endif +} + +partition Step4 { +:select **new candidate modules** from request modules; +:run all candidate modules; +} + +partition Step5 { +:decide the priority order of execution among **candidate modules stack**; +} + +partition Step6 { +if(approved module exists in candidate modules stack) then (TRUE) +:move **approved module** to approved modules stack; +else (NO) +stop +note right +break from the loop +end note +endif +} + +endwhile +-[hidden]-> +detach + +@enduml +``` + +```c++ + while (rclcpp::ok()) { + /** + * STEP1: get approved modules' output + */ + const auto approved_modules_output = runApprovedModules(data); + + /** + * STEP2: check modules that need to be launched + */ + const auto request_modules = getRequestModules(approved_modules_output); + + /** + * STEP3: if there is no module that need to be launched, return approved modules' output + */ + if (request_modules.empty()) { + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return approved_modules_output; + } + + /** + * STEP4: if there is module that should be launched, execute the module + */ + const auto [highest_priority_module, candidate_modules_output] = + runRequestModules(request_modules, data, approved_modules_output); + if (!highest_priority_module) { + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return approved_modules_output; + } + + /** + * STEP5: if the candidate module's modification is NOT approved yet, return the result. + * NOTE: the result is output of the candidate module, but the output path don't contains path + * shape modification that needs approval. On the other hand, it could include velocity profile + * modification. + */ + if (highest_priority_module->isWaitingApproval()) { + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return candidate_modules_output; + } + + /** + * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ + */ + addApprovedModule(highest_priority_module); + clearCandidateModules(); + } +``` + +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/src/planner_manager.cpp#L66-L111) + +## Priority of execution request + +Compare priorities parameter among sub-managers to determine the order of execution based on config. Therefore, the priority between sub-modules does **NOT** change at runtime. + +```c++ + /** + * @brief swap the modules order based on it's priority. + * @param modules. + * @details for now, the priority is decided in config file and doesn't change runtime. + */ + void sortByPriority(std::vector & modules) const + { + // TODO(someone) enhance this priority decision method. + std::sort(modules.begin(), modules.end(), [this](auto a, auto b) { + return getManager(a)->getPriority() < getManager(b)->getPriority(); + }); + } +``` + +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L239-L250) + +In the future, however, we are considering having the priorities change dynamically depending on the situation in order to achieve more complex use cases. + +## How to decide which request modules to run? + +On this manager, it is possible that multiple scene modules may request path modification at same time. In that case, the modules to be executed as candidate module is determined in the following order. + +### Step1 + +Push back the modules that make a request to `request_modules`. + +![request_step1](../image/manager/request_step1.svg) + +### Step2 + +Check approved modules stack, and remove non-executable modules from`request_modules` based on the following condition. + +- **Condition A.** approved module stack is empty. +- **Condition B.** all modules in approved modules stack support simultaneous execution as approved module (`enable_simultaneous_execution_as_approved_module` is `true`). +- **Condition C.** the request module supports simultaneous execution as approved module. + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +start + +while () + +if (Condition A is satisfied) then (YES) +stop +else (NO) +if (Condition B is satisfied) then (YES) +if (Condition C is satisfied) then (YES) +else (NO) +:remove the request modules; +endif +else (NO) +:remove all request modules; +stop +endif +endif + +endwhile +-[hidden]-> +detach + +@enduml +``` + +Executable or not: + +| Condition A | Condition B | Condition C | Executable as candidate modules? | +| :---------: | :---------: | :---------: | :------------------------------: | +| YES | - | YES | **YES** | +| YES | - | NO | **YES** | +| NO | YES | YES | **YES** | +| NO | YES | NO | NO | +| NO | NO | YES | NO | +| NO | NO | NO | NO | + +If a module that doesn't support simultaneous execution exists in approved modules stack (**NOT** satisfy Condition B), no more modules can be added to the stack, and therefore none of the modules can be executed as candidate. + +For example, if approved module's setting of `enable_simultaneous_execution_as_approved_module` is **ENABLE**, then only modules whose the setting is **ENABLE** proceed to the next step. + +![request_step2](../image/manager/request_step2.svg) + +Other examples: + +| Process | Description | +| :------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ![request_step2-2](../image/manager/request_step2-2.svg) | If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. | +| ![request_step2-3](../image/manager/request_step2-3.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. | + +### Step3 + +Sort `request_modules` by priority. + +![request_step3](../image/manager/request_step3.svg) + +### Step4 + +Check and pick up executable modules as candidate in order of priority based on the following conditions. + +- **Condition A.** candidate module stack is empty. +- **Condition B.** all modules in candidate modules stack support simultaneous execution as candidate module (`enable_simultaneous_execution_as_candidate_module` is `true`). +- **Condition C.** the request module supports simultaneous execution as candidate module. + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +start + +while () + +if (Condition A is satisfied) then (YES) +:push back the request module to candidate module stack; +else (NO) +if (Condition B is satisfied) then (YES) +if (Condition C is satisfied) then (YES) +:push back the request module to candidate module stack; +else (NO) +stop +endif +else (NO) +stop +endif +endif + +endwhile +-[hidden]-> +detach + +@enduml +``` + +Executable or not: + +| Condition A | Condition B | Condition C | Executable as candidate modules? | +| :---------: | :---------: | :---------: | :------------------------------: | +| YES | - | YES | **YES** | +| YES | - | NO | **YES** | +| NO | YES | YES | **YES** | +| NO | YES | NO | NO | +| NO | NO | YES | NO | +| NO | NO | NO | NO | + +For example, if the highest priority module's setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE**, then all modules after the second priority are discarded. + +![request_step4](../image/manager/request_step4.svg) + +Other examples: + +| Process | Description | +| :------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ![request_step4-2](../image/manager/request_step4-2.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. | +| ![request_step4-3](../image/manager/request_step4-3.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. | + +### Step5 + +Run all candidate modules. + +![request_step5](../image/manager/request_step5.svg) + +## How to decide which module's output to use? + +Sometimes, multiple candidate modules are running simultaneously. + +![multiple_candidates](../image/manager/multiple_candidates.svg) + +In this case, the manager selects a candidate modules which output path is used as `behavior_path_planner` output by approval condition in the following rules. + +- **Rule A.** Regardless of the priority in the sub-manager (`priority`), approved modules always have a higher priority than unapproved modules. +- **Rule B.** If the approval status is the same, sort according to the sub-manager's priority. + +| Module A's approval condition | Module A's priority | Module B's approval condition | Module B's priority | Final priority | +| :---------------------------: | :-----------------: | :---------------------------: | :-----------------: | :---------------------: | +| Approved | 1 | Approved | 99 | Module A > Module B | +| Approved | 1 | Not approved | 99 | Module A > Module B | +| Not approved | 1 | Approved | 99 | **Module B > Module A** | +| Not approved | 1 | Not approved | 99 | Module A > Module B | + +!!! note + + The smaller the number is, the higher the priority is. + +
+ ![module_select](../image/manager/module_select.svg){width=1000} +
module priority
+
+ +![output_module](../image/manager/output_module.svg) + +Additionally, the manager moves the highest priority module to approved modules stack if it is already approved. + +## Scene module unregister process + +The manager removes expired module in approved modules stack based on the module's status. + +### Waiting approval modules + +If one module requests multiple path changes, the module may be back to waiting approval condition again. In this case, the manager moves the module to candidate modules stack. If there are some modules that was pushed back to approved modules stack later than the waiting approved module, it is also removed from approved modules stack. + +This is because module C is planning output path with the output of module B as input, and if module B is removed from approved modules stack and the input of module C changes, the output path of module C may also change greatly, and the output path will be unstable. + +As a result, the module A's output is used as approved modules stack. + +![waiting_approve](../image/manager/waiting_approve.svg) + +### Failure modules + +The failure modules return the status `ModuleStatus::FAILURE`. The manager removes the module from approved modules stack as well as waiting approval modules, but the failure module is not moved to candidate modules stack. + +As a result, the module A's output is used as approved modules stack. + +![failure_modules](../image/manager/failure_modules.svg) + +### Succeeded modules + +The succeeded modules return the status `ModuleStatus::SUCCESS`. The manager removes those modules based on **Last In First Out** policy. In other words, if a module added later to approved modules stack is still running (is in `ModuleStatus::RUNNING`), the manager doesn't remove the succeeded module. The reason for this is the same as in removal for waiting approval modules, and is to prevent sudden changes of the running module's output. + +![success_modules_remove](../image/manager/success_modules_remove.svg) + +![success_modules_skip](../image/manager/success_modules_skip.svg) + +As an exception, if **Lane Change** module returns status `ModuleStatus::SUCCESS`, the manager doesn't remove any modules until all modules is in status `ModuleStatus::SUCCESS`. This is because when the manager removes the **Lane Change** (normal LC, external LC, avoidance by LC) module as succeeded module, the manager updates the information of the lane Ego is currently driving in, so root reference path (= module A's input path) changes significantly at that moment. + +![lane_change_remove](../image/manager/lane_change_remove.svg) + +![lane_change_skip](../image/manager/lane_change_skip.svg) + +When the manager removes succeeded modules, the last added module's output is used as approved modules stack. + +## Reference path generation + +The root reference path is generated from the centerline of the **lanelet sequence** that obtained from the **root lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running. + +
+ ![root_generation](../image/manager/root_generation.svg){width=500} +
root reference path generation
+
+ +The root lanelet is the closest lanelet within the route, and the update timing is based on Ego's operation mode state. + +- the state is `OperationModeState::AUTONOMOUS`: Update only when the ego moves to right or left lane by lane change module. +- the state is **NOT** `OperationModeState::AUTONOMOUS`: Update at the beginning of every planning cycle. + +![root_lanelet](../image/manager/root_lanelet.svg) + +The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow. + +For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** update **root lanelet** even if the avoidance maneuver is finished. + +![avoidance](../image/manager/avoidance.svg) + +On the other hand, if the lane change is successful, the manager updates **root lanelet** because the lane that Ego should follow changes. + +![lane_change](../image/manager/lane_change.svg) + +In addition, while manual driving, the manager always updates **root lanelet** because the pilot may move to an adjacent lane regardless of the decision of the autonomous driving system. + +```c++ + /** + * @brief get reference path from root_lanelet_ centerline. + * @param planner data. + * @return reference path. + */ + BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const + { + const auto & route_handler = data->route_handler; + const auto & pose = data->self_odometry->pose.pose; + const auto p = data->parameters; + + constexpr double extra_margin = 10.0; + const auto backward_length = + std::max(p.backward_path_length, p.backward_path_length + extra_margin); + + const auto lanelet_sequence = route_handler->getLaneletSequence( + root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); + + lanelet::ConstLanelet closest_lane{}; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + return utils::getReferencePath(closest_lane, data); + } + + if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return utils::getReferencePath(closest_lane, data); + } + + return {}; // something wrong. + } +``` + +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L202-L227) + +## Drivable area generation + +!!! warning + + Under Construction + +## Turn signal management + +!!! warning + + Under Construction diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md new file mode 100644 index 0000000000000..2d8ec7a6b2de3 --- /dev/null +++ b/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md @@ -0,0 +1,63 @@ +# Safety Check Utils + +Safety check function checks if the given path will collide with a given target object. + +## Purpose / Role + +In the behavior path planner, certain modules (e.g., lane change) need to perform collision checks to ensure the safe navigation of the ego vehicle. These utility functions assist the user in conducting safety checks with other road participants. + +### Assumptions + +The safety check module is based on the following assumptions: + +1. Users must provide the position, velocity, and shape of both the ego and target objects to the utility functions. +2. The yaw angle of each point in the predicted path of both the ego and target objects should point to the next point in the path. +3. The safety check module uses RSS distance to determine the safety of a potential collision with other objects. + +### Limitations + +Currently the yaw angle of each point of predicted paths of a target object does not point to the next point. Therefore, the safety check function might returns incorrect result for some edge case. + +### Inner working / Algorithm + +The flow of the safety check algorithm is described in the following explanations. + +![safety_check_flow](../image/safety_check/safety_check_flow.drawio.svg) + +Here we explain each step of the algorithm flow. + +#### 1. Get pose of the target object at a given time + +For the first step, we obtain the pose of the target object at a given time. This can be done by interpolating the predicted path of the object. + +#### 2. Check overlap + +With the interpolated pose obtained in the step.1, we check if the object and ego vehicle overlaps at a given time. If they are overlapped each other, the given path is unsafe. + +#### 3. Get front object + +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/safety_check/front_object.drawio.svg) + +#### 4. Calculate RSS distance + +After we find which vehicle is running ahead of the other vehicle, we start to compute the RSS distance. With the reaction time $t_{reaction}$ and safety time margin $t_{margin}$, RSS distance can be described as: + +$$ +rss_{dist} = v_{rear} (t_{reaction} + t_{margin}) + \frac{v_{rear}^2}{2|a_{rear, decel}|} - \frac{v_{front}^2}{2|a_{front, decel|}} +$$ + +where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. + +#### 5. Create extended ego and target object polygons + +In this step, we compute extended ego and target object polygons. The extended polygons can be described as: + +![extended_polygons](../image/safety_check/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 + +#### 6. Check overlap + +Similar to the previous step, we check the overlap of the extended rear object polygon and front object polygon. If they are overlapped each other, we regard it as the unsafe situation. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md index ede77e8a38e12..8b64db997aec6 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md @@ -53,7 +53,7 @@ When turning on the blinker, it decides whether or not to turn on the specified ![activation_distance](../image/turn_signal_decider/activation_distance.drawio.svg) -For left turn, right turn, avoidance, lane change, pull over and pull out, we define these two sections, which are elaborated in the following part. +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. #### 1. Left and Right turn @@ -141,7 +141,7 @@ Second section ![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg) -#### 5. Pull over +#### 5. Goal Planner - desired start point `v * turn_signal_search_time` meters before the start point of the pull over path. diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg new file mode 100644 index 0000000000000..e0741781998bf --- /dev/null +++ b/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg @@ -0,0 +1,145 @@ + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ max lateral offset to avoid +
+
+
+ + max lateral o... + + + + + + +
+
+
+ lateral offset from obstacle +
+
+
+ + lateral offse... + + + + + + +
+
+
+ reference path +
+
+
+ + reference path + + + + + + +
+
+
+ obstacle polygon to extract from the drivable area +
+
+
+ + obstacle polygon to extrac... + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg b/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg new file mode 100644 index 0000000000000..d9b4f6dc35bf1 --- /dev/null +++ b/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg @@ -0,0 +1,235 @@ + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + +
+
+
+ reference path +
+
+
+ + reference path + + + + + + + + + + + +
+
+
+ time to collision (cropped) +
+ * +
+ obstacle velocity +
+
+
+ + time to collision (cropped)... + + + + + + + + + + + + + +
+
+
+ margin time to avoid +
+ * +
+ obstacle velocity +
+
+
+ + margin time to avoid... + + + + + + + + + + + + + +
+
+
+ time to collision (not cropped) +
+ * +
+ obstacle velocity +
+
+
+ + time to collision (not cropped... + + + + + + + + + + + + + +
+
+
+ margin time to avoid +
+ * +
+ obstacle velocity +
+
+
+ + margin time to avoid... + + + + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg b/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg new file mode 100644 index 0000000000000..41d0a4b5b8983 --- /dev/null +++ b/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg @@ -0,0 +1,190 @@ + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + +
+
+
+ reference path +
+
+
+ + reference path + + + + + + + + + + + + + + +
+
+
+ vehicle length +
+
+
+ + vehicle length + + + + + + + + + + + + +
+
+
+ margin time to avoid +
+ * +
+ obstacle velocity +
+
+
+ + margin time to avoid... + + + + + + +
+
+
+ Ignore this obstacle +
+ since TTC is negative +
+
+
+ + Ignore this obstacle... + + + + + + +
+
+
+ Consider this obstacle +
+ since TTC is positive +
+
+
+ + Consider this obstacle... + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-collision_check.png b/planning/behavior_path_planner/image/lane_change/lane_change-collision_check.png deleted file mode 100644 index 7fc897471716e..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change/lane_change-collision_check.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-collision_check_parked_vehicle.png b/planning/behavior_path_planner/image/lane_change/lane_change-collision_check_parked_vehicle.png deleted file mode 100644 index 32ab7b34b7966..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change/lane_change-collision_check_parked_vehicle.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/manager/approved_modules_stack.svg b/planning/behavior_path_planner/image/manager/approved_modules_stack.svg new file mode 100644 index 0000000000000..16185ed2a5f62 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/approved_modules_stack.svg @@ -0,0 +1,131 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + +
+
+
+ input +
+
+
+ + input + + + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/avoidance.svg b/planning/behavior_path_planner/image/manager/avoidance.svg new file mode 100644 index 0000000000000..b61231aa69b17 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/avoidance.svg @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ ego's driving lane +
+
+
+ + ego's driving lane + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/candidate_modules_stack.svg b/planning/behavior_path_planner/image/manager/candidate_modules_stack.svg new file mode 100644 index 0000000000000..c717f427107f9 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/candidate_modules_stack.svg @@ -0,0 +1,131 @@ + + + + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ input +
+
+
+ + input + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/example_behavior.svg b/planning/behavior_path_planner/image/manager/example_behavior.svg new file mode 100644 index 0000000000000..5213154714a2d --- /dev/null +++ b/planning/behavior_path_planner/image/manager/example_behavior.svg @@ -0,0 +1,397 @@ + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + +
+
+
+

+ Execute avoidance module during lane change maneuver. +

+
+
+
+ + Execute avoidance module during lane change maneuver. + + + + + + + + + +
+
+
+ output path +
+
+
+ + output p... + + + + + + +
+
+
+ candidate path +
+
+
+ + candidat... + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + + + + + + +
+
+
+ want to move next lane... +
+
+
+ + want to move nex... + + + + + + + + + + + +
+
+
+ want to avoid the obstacle... +
+
+
+ + want to avoid th... + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + +
+
+
+ found obstacle on lane change target lane... +
+
+
+ + found obstacle o... + + + + + + +
+
+
+ avoidance module starts running. +
+
+
+ + avoidance module starts running. + + + + + + +
+
+
+ lane change module is still running. +
+
+
+ + lane change module is still running. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/failure_modules.svg b/planning/behavior_path_planner/image/manager/failure_modules.svg new file mode 100644 index 0000000000000..78653e8f9170b --- /dev/null +++ b/planning/behavior_path_planner/image/manager/failure_modules.svg @@ -0,0 +1,384 @@ + + + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ ModuleStatus::FAILURE +
+
+
+ + ModuleStatus::FAILURE + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/lane_change.svg b/planning/behavior_path_planner/image/manager/lane_change.svg new file mode 100644 index 0000000000000..b0a01d6b9daae --- /dev/null +++ b/planning/behavior_path_planner/image/manager/lane_change.svg @@ -0,0 +1,212 @@ + + + + + + + + + + + + + + +
+
+
+ ego's driving lane +
+
+
+ + ego's driving lane + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + +
+
+
+ before lane change +
+
+
+ + before lane change + + + + + + +
+
+
+ after lane change +
+
+
+ + after lane change + + + + + + + +
+
+
+ root lanelet +
+
+
+ + root lanelet + + + + + + + +
+
+
+ root lanelet (UPDATED) +
+
+
+ + root lanelet (UPDATED) + + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/lane_change_remove.svg b/planning/behavior_path_planner/image/manager/lane_change_remove.svg new file mode 100644 index 0000000000000..26dd05637b5da --- /dev/null +++ b/planning/behavior_path_planner/image/manager/lane_change_remove.svg @@ -0,0 +1,581 @@ + + + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + +
+
+
+ Lane Change +
+
+
+ + Lane Change + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ Lane Change +
+
+
+ + Lane Change + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + + + +
+
+
+ Update driving lane data. +
+
+
+ + Update driving lane data. + + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/lane_change_skip.svg b/planning/behavior_path_planner/image/manager/lane_change_skip.svg new file mode 100644 index 0000000000000..c3741351798c1 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/lane_change_skip.svg @@ -0,0 +1,603 @@ + + + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + +
+
+
+ Lane Change +
+
+
+ + Lane Change + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + + +
+
+
+ ModuleStatus::RUNNING +
+
+
+ + ModuleStatus::RUNNING + + + + + + +
+
+
+ ModuleStatus::RUNNING +
+
+
+ + ModuleStatus::RUNNING + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + + +
+
+
+ Lane Change +
+
+
+ + Lane Change + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ ModuleStatus::RUNNING +
+
+
+ + ModuleStatus::RUNNING + + + + + + +
+
+
+ ModuleStatus::RUNNING +
+
+
+ + ModuleStatus::RUNNING + + + + + + +
+
+
+ Do nothing. +
+
+
+ + Do nothing. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/manager_overview.svg b/planning/behavior_path_planner/image/manager/manager_overview.svg new file mode 100644 index 0000000000000..b17b98285346d --- /dev/null +++ b/planning/behavior_path_planner/image/manager/manager_overview.svg @@ -0,0 +1,814 @@ + + + + + + + + + +
+
+
+ behavior path planner (node) +
+
+
+ + behavior path planner (node) + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ planner +
+ data +
+
+
+ + planner... + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + +
+
+
+ candidate path +
+
+
+ + candidate path + + + + + + +
+
+
+ scene module C (highest priority) +
+
+
+ + scene module C (highest priority) + + + + + + + +
+
+
+ candidate path +
+
+
+ + candidate path + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + +
+
+
+ candidate path +
+
+
+ + candidate path + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + +
+
+
+ candidate module output +
+
+
+ + candidate... + + + + + + + +
+
+
+ RTC cooperate status +
+
+
+ + RTC cooperate status + + + + + + + +
+
+
+ candidate module output +
+
+
+ + candidate... + + + + + + + +
+
+
+ RTC cooperate status +
+
+
+ + RTC cooperate status + + + + + + + +
+
+
+ candidate module output +
+
+
+ + candidate... + + + + + + + +
+
+
+ RTC cooperate status +
+
+
+ + RTC cooperate status + + + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + +
+
+
+ reference path generated from the centerline of the driving lane +
+
+
+ + reference path generat... + + + + + + +
+
+
+ module +
+ data +
+
+
+ + module... + + + + + + + +
+
+
+ path +
+
+
+ + path + + + + + + + + + +
+
+
+ planner data +
+
+
+ + planner data + + + + + + + +
+
+
+ route +
+
+
+ + route + + + + + + + +
+
+
+ map +
+
+
+ + map + + + + + + + +
+
+
+ odometry +
+
+
+ + odometry + + + + + + + +
+
+
+ dynamic objects +
+
+
+ + dynamic... + + + + + + + +
+
+
+ occupancy grid +
+
+
+ + occupanc... + + + + + + + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + +
+
+
+ post +
+ process +
+
+
+ + post... + + + + + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/module_select.svg b/planning/behavior_path_planner/image/manager/module_select.svg new file mode 100644 index 0000000000000..5033a54e480e6 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/module_select.svg @@ -0,0 +1,168 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + condition: + APPROVED +
+ priority: + + 1 +
+ +
+
+
+ + scene module Bcondition: APPROVED... + + + + + + +
+
+
+ scene module D +
+ condition: WAITING APPROVAL +
+ priority: + 1 +
+
+
+ + scene module D... + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + +
+
+
+ scene module C + +
+ + condition: + APPROVED +
+ + priority: 99 +
+ +
+
+
+ + scene module Ccondition: APPROVED... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/multiple_candidates.svg b/planning/behavior_path_planner/image/manager/multiple_candidates.svg new file mode 100644 index 0000000000000..650b0b76548d1 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/multiple_candidates.svg @@ -0,0 +1,248 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/output_module.svg b/planning/behavior_path_planner/image/manager/output_module.svg new file mode 100644 index 0000000000000..ec3079114f4ea --- /dev/null +++ b/planning/behavior_path_planner/image/manager/output_module.svg @@ -0,0 +1,226 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + +
+
+
+ Highest priority +
+
+
+ + Highest priority + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/process_step1.svg b/planning/behavior_path_planner/image/manager/process_step1.svg new file mode 100644 index 0000000000000..c8bc12a070d0d --- /dev/null +++ b/planning/behavior_path_planner/image/manager/process_step1.svg @@ -0,0 +1,322 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + +
+
+
+ The manager run all approved modules. +
+
+
+ + The manager run all approved modules. + + + + + + +
+
+
+ Get approved modules' output. +
+
+
+ + Get approved modules... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/process_step2.svg b/planning/behavior_path_planner/image/manager/process_step2.svg new file mode 100644 index 0000000000000..b387c73e3165f --- /dev/null +++ b/planning/behavior_path_planner/image/manager/process_step2.svg @@ -0,0 +1,403 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ planner +
+ data +
+
+
+ + planner... + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + +
+
+
+ module +
+ data +
+
+
+ + module... + + + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ The manager pass latest planner data and approved modules' output. +
+
+
+ + The manager pass latest plan... + + + + + + +
+
+
+ The manager receives module's execution request and pointer. +
+
+
+ + The manager receives module'... + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/process_step4.svg b/planning/behavior_path_planner/image/manager/process_step4.svg new file mode 100644 index 0000000000000..06536181e6334 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/process_step4.svg @@ -0,0 +1,377 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + +
+
+
+ Launch +
+
+
+ + Launch + + + + + + +
+
+
+ Launch +
+
+
+ + Launch + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/process_step5.svg b/planning/behavior_path_planner/image/manager/process_step5.svg new file mode 100644 index 0000000000000..1d886e47d9ad4 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/process_step5.svg @@ -0,0 +1,429 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D-1 +
+
+
+ + scene module D-1 + + + + + + +
+
+
+ scene module D-2 +
+
+
+ + scene module D-2 + + + + + + + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + +
+
+
+ Highest priority +
+
+
+ + Highest priority + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/process_step6.svg b/planning/behavior_path_planner/image/manager/process_step6.svg new file mode 100644 index 0000000000000..cf0001737acd1 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/process_step6.svg @@ -0,0 +1,377 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D-1 +
+
+
+ + scene module D-1 + + + + + + + +
+
+
+ scene module D-2 +
+
+
+ + scene module D-2 + + + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ Move approved +
+ modules stack +
+
+
+ + Move appr... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step1.svg b/planning/behavior_path_planner/image/manager/request_step1.svg new file mode 100644 index 0000000000000..a3e3489f0cb7f --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step1.svg @@ -0,0 +1,386 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + +
+
+
+ launch +
+
+
+ + launch + + + + + + +
+
+
+ launch +
+
+
+ + launch + + + + + + +
+
+
+ launch +
+
+
+ + launch + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step2-2.svg b/planning/behavior_path_planner/image/manager/request_step2-2.svg new file mode 100644 index 0000000000000..f93ae15728181 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step2-2.svg @@ -0,0 +1,229 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module C +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module C... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ EMPTY +
+
+
+ + EMPTY + + + + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step2-3.svg b/planning/behavior_path_planner/image/manager/request_step2-3.svg new file mode 100644 index 0000000000000..38ef0df67f3f2 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step2-3.svg @@ -0,0 +1,232 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module C +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module C... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module A... + + + + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step2.svg b/planning/behavior_path_planner/image/manager/request_step2.svg new file mode 100644 index 0000000000000..b51d1a62cd35d --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step2.svg @@ -0,0 +1,231 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module C +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module C... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module A... + + + + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step3.svg b/planning/behavior_path_planner/image/manager/request_step3.svg new file mode 100644 index 0000000000000..631cf38245b17 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step3.svg @@ -0,0 +1,126 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ priority: + HIGHEST +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module D +
+ priority: LOWEST +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step4-2.svg b/planning/behavior_path_planner/image/manager/request_step4-2.svg new file mode 100644 index 0000000000000..d35c8c60ec78f --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step4-2.svg @@ -0,0 +1,244 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Bsimultaneous execution as... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as candidate: + DISABLE +
+
+
+ + scene module D... + + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + +
+
+
+ scene module C + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Csimultaneous execution as... + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step4-3.svg b/planning/behavior_path_planner/image/manager/request_step4-3.svg new file mode 100644 index 0000000000000..78240c9f5a307 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step4-3.svg @@ -0,0 +1,245 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Bsimultaneous execution as... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as candidate: ENABLE +
+
+
+ + scene module D... + + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + +
+
+
+ scene module C + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Csimultaneous execution as... + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step4.svg b/planning/behavior_path_planner/image/manager/request_step4.svg new file mode 100644 index 0000000000000..c4b1d8d385122 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step4.svg @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + simultaneous execution as candidate: + + DISABLE +
+ +
+
+
+ + scene module Bsimultaneous execution as... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as candidate: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step5.svg b/planning/behavior_path_planner/image/manager/request_step5.svg new file mode 100644 index 0000000000000..54b5b7840995b --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step5.svg @@ -0,0 +1,183 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + + + + +
+
+
+ executing +
+
+
+ + executing + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/root_generation.svg b/planning/behavior_path_planner/image/manager/root_generation.svg new file mode 100644 index 0000000000000..a944e5c348f2c --- /dev/null +++ b/planning/behavior_path_planner/image/manager/root_generation.svg @@ -0,0 +1,93 @@ + + + + + + + + + + +
+
+
+ root lanelet +
+
+
+ + root lanelet + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + + +
+
+
+ root reference path +
+
+
+ + root reference path + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/root_lanelet.svg b/planning/behavior_path_planner/image/manager/root_lanelet.svg new file mode 100644 index 0000000000000..802eb1613e77b --- /dev/null +++ b/planning/behavior_path_planner/image/manager/root_lanelet.svg @@ -0,0 +1,166 @@ + + + + + + + + + + +
+
+
+ root lanelet +
+
+
+ + root lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + + +
+
+
+ root reference path +
+
+
+ + root refe... + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + +
+
+
+ lanelet sequence +
+
+
+ + lanelet sequence + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/scene_module.svg b/planning/behavior_path_planner/image/manager/scene_module.svg new file mode 100644 index 0000000000000..8e6c2f1e81c81 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/scene_module.svg @@ -0,0 +1,177 @@ + + + + + + + + + + +
+
+
+ output path +
+
+
+ + output... + + + + + + +
+
+
+ scene module +
+
+
+ + scene module + + + + + + + +
+
+
+ rtc command +
+
+
+ + rtc com... + + + + + + + +
+
+
+ candidate path +
+
+
+ + candida... + + + + + + + +
+
+
+ input path +
+
+
+ + input p... + + + + + + + +
+
+
+ planner data +
+
+
+ + planner... + + + + + + + +
+
+
+ rtc cooperate status +
+
+
+ + rtc coo... + + + + + + + +
+
+
+ reference path +
+
+
+ + referen... + + + + + + + +
+
+
+ debug marker +
+
+
+ + debug m... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/sub_managers.svg b/planning/behavior_path_planner/image/manager/sub_managers.svg new file mode 100644 index 0000000000000..5a32c019d8648 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/sub_managers.svg @@ -0,0 +1,220 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ planner +
+ data +
+
+
+ + planner... + + + + + + +
+
+
+ module +
+ data +
+
+
+ + module... + + + + + + +
+
+
+ The manager pass latest planner data +
+ and approved modules output. +
+
+
+ + The manager pass latest planner data... + + + + + + +
+
+
+ The manager receives execution requests and scene modules pointer. +
+
+
+ + The manager receives execution request... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/success_modules_remove.svg b/planning/behavior_path_planner/image/manager/success_modules_remove.svg new file mode 100644 index 0000000000000..7b85f38c21e92 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/success_modules_remove.svg @@ -0,0 +1,377 @@ + + + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/success_modules_skip.svg b/planning/behavior_path_planner/image/manager/success_modules_skip.svg new file mode 100644 index 0000000000000..d54de3ddcc148 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/success_modules_skip.svg @@ -0,0 +1,464 @@ + + + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ ModuleStatus::RUNNING +
+
+
+ + ModuleStatus::RUNNING + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ ModuleStatus::RUNNING +
+
+
+ + ModuleStatus::RUNNING + + + + + + +
+
+
+ ModuleStatus::SUCCESS +
+
+
+ + ModuleStatus::SUCCESS + + + + + + +
+
+
+ Do nothing. +
+
+
+ + Do nothing. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/waiting_approve.svg b/planning/behavior_path_planner/image/manager/waiting_approve.svg new file mode 100644 index 0000000000000..41345df4f3fb1 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/waiting_approve.svg @@ -0,0 +1,403 @@ + + + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ Waiting approval +
+
+
+ + Waiting approval + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + + +
+
+
+ Move to candidate modules stack. +
+
+
+ + Move to candidate mod... + + + + + + + + +
+
+
+ Remove. +
+
+
+ + Remove. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg b/planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg new file mode 100644 index 0000000000000..8411bbed202e1 --- /dev/null +++ b/planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + +
+
+
+ RSS distance +
+
+
+ + RSS distance + + + + + + + + +
+
+
+ + lateral margin +
+ +
+
+
+ + lateral margin + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/safety_check/front_object.drawio.svg b/planning/behavior_path_planner/image/safety_check/front_object.drawio.svg new file mode 100644 index 0000000000000..4a3a9d3bfb2c1 --- /dev/null +++ b/planning/behavior_path_planner/image/safety_check/front_object.drawio.svg @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + +
+
+
+ Object +
+
+
+ + Object + + + + + + + + + +
+
+
+ + Ego Arclength + +
+
+
+ + Ego Arclength + + + + + + + + +
+
+
+ + + Object Arclength + + +
+
+
+ + Object Arclength + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg b/planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg new file mode 100644 index 0000000000000..c61c379d66d8a --- /dev/null +++ b/planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg @@ -0,0 +1,249 @@ + + + + + + + + + + + + +
+
+
+ Get pose of the target object at a given time +
+
+
+ + Get pose of the target object at a given time + + + + + + + + +
+
+
+ + Get front object +
+ (Ego or target vehicle) + +
+
+
+ + Get front object... + + + + + + +
+
+
+ Check overlap +
+
+
+ + Check overlap + + + + + + +
+
+
+ Calculate RSS distance +
+
+
+ + Calculate RSS distance + + + + + + +
+
+
+ Create ego and object extended polygons +
+
+
+ + Create ego and object extended polygons + + + + + + +
+
+
+ Check overlap +
+
+
+ + Check overlap + + + + + + + + + + + + + +
+
+
+ No +
+
+
+ + No + + + + + +
+
+
+ Yes +
+
+
+ + Yes + + + + + +
+
+
+ Unsafe +
+
+
+ + Unsafe + + + + + +
+
+
+ Unsafe +
+
+
+ + Unsafe + + + + + + + +
+
+
+ Yes +
+
+
+ + Yes + + + + + + + +
+
+
+ No +
+
+
+ + No + + + + + +
+
+
+ Safe +
+
+
+ + Safe + + + + + + + Text is not SVG - cannot display + + + 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 562a71fb00f6b..29ed73b9374e6 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 @@ -21,30 +21,27 @@ #ifdef USE_OLD_ARCHITECTURE #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp" -#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" -#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #else #include "behavior_path_planner/planner_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/manager.hpp" #include "behavior_path_planner/scene_module/pull_out/manager.hpp" -#include "behavior_path_planner/scene_module/pull_over/manager.hpp" #include "behavior_path_planner/scene_module/side_shift/manager.hpp" #endif #include "behavior_path_planner/steering_factor_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "behavior_path_planner/utils/pull_out/pull_out_parameters.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" #include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" @@ -76,6 +73,7 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -137,8 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool has_received_map_{false}; bool has_received_route_{false}; - TurnSignalDecider turn_signal_decider_; - std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ std::mutex mutex_map_; // mutex for has_received_map_ and map_ptr_ @@ -150,11 +146,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node // parameters std::shared_ptr avoidance_param_ptr_; std::shared_ptr avoidance_by_lc_param_ptr_; + std::shared_ptr dynamic_avoidance_param_ptr_; std::shared_ptr side_shift_param_ptr_; std::shared_ptr lane_change_param_ptr_; - std::shared_ptr lane_following_param_ptr_; std::shared_ptr pull_out_param_ptr_; - std::shared_ptr pull_over_param_ptr_; + std::shared_ptr goal_planner_param_ptr_; BehaviorPathPlannerParameters getCommonParam(); @@ -163,10 +159,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node #endif AvoidanceParameters getAvoidanceParam(); + DynamicAvoidanceParameters getDynamicAvoidanceParam(); LaneChangeParameters getLaneChangeParam(); - LaneFollowingParameters getLaneFollowingParam(); SideShiftParameters getSideShiftParam(); - PullOverParameters getPullOverParam(); + GoalPlannerParameters getGoalPlannerParam(); PullOutParameters getPullOutParam(); AvoidanceByLCParameters getAvoidanceByLCParam( const std::shared_ptr & avoidance_param, @@ -184,12 +180,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onLateralOffset(const LateralOffset::ConstSharedPtr msg); SetParametersResult onSetParam(const std::vector & parameters); - /** - * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point. - */ - PathWithLaneId modifyPathForSmoothGoalConnection( - const PathWithLaneId & path, - const std::shared_ptr & planner_data) const; // (TODO) move to util OnSetParametersCallbackHandle::SharedPtr m_set_param_res; /** @@ -210,12 +200,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & planner_manager); #endif - /** - * @brief skip smooth goal connection - */ - bool skipSmoothGoalConnection( - const std::vector> & statuses) const; - bool keepInputPoints(const std::vector> & statuses) const; /** @@ -233,7 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish steering factor from intersection */ - void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); + void publish_steering_factor( + const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal); /** * @brief publish left and right bound diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp index ef26ff81d61d9..f179b90139705 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp @@ -19,6 +19,9 @@ #include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include +#include + #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 2111760083358..59b903e1995e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_ #include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include @@ -43,6 +44,7 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; @@ -76,22 +78,25 @@ struct DrivableLanes lanelet::ConstLanelets middle_lanes; }; -struct TurnSignalInfo +// NOTE: To deal with some policies about drivable area generation, currently DrivableAreaInfo is +// quite messy. Needs to be refactored. +struct DrivableAreaInfo { - TurnSignalInfo() + struct Obstacle { - turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; - hazard_signal.command = HazardLightsCommand::NO_COMMAND; - } - - // desired turn signal - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - - geometry_msgs::msg::Pose desired_start_point; - geometry_msgs::msg::Pose desired_end_point; - geometry_msgs::msg::Pose required_start_point; - geometry_msgs::msg::Pose required_end_point; + geometry_msgs::msg::Pose pose; + tier4_autoware_utils::Polygon2d poly; + bool is_left; + }; + std::vector drivable_lanes{}; + std::vector obstacles{}; // obstacles to extract from the drivable area + bool enable_expanding_hatched_road_markings{false}; + + // temporary only for pull over's freespace planning + double drivable_margin{0.0}; + + // temporary only for side shift + bool is_already_expanded{false}; }; struct BehaviorModuleOutput @@ -108,8 +113,9 @@ struct BehaviorModuleOutput std::optional modified_goal{}; - // drivable lanes - std::vector drivable_lanes; + // drivable area info to create drivable area + // NOTE: Drivable area in the path is generated at last from drivable_area_info. + DrivableAreaInfo drivable_area_info; }; struct CandidateOutput @@ -133,11 +139,23 @@ struct PlannerData OperationModeState::ConstSharedPtr operation_mode{}; PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; + std::optional prev_modified_goal{}; lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable TurnSignalDecider turn_signal_decider; + + TurnIndicatorsCommand getTurnSignal( + const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info) + { + const auto & current_pose = self_odometry->pose.pose; + const auto & current_vel = self_odometry->twist.twist.linear.x; + return turn_signal_decider.getTurnSignal( + route_handler, path, turn_signal_info, current_pose, current_vel, parameters); + } + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp index 51de1d2d6318d..9d39fa32417f5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp @@ -58,18 +58,12 @@ MarkerArray createAvoidLineMarkerArray( MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); -MarkerArray createAvoidableTargetObjectsMarkerArray( - const ObjectDataArray & objects, std::string && ns); +MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createUnavoidableTargetObjectsMarkerArray( - const ObjectDataArray & objects, std::string && ns); - -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); +MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); -MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); - MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index b2a5bdb096170..6c65126545120 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -15,8 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ +#include #include +#include +#include +#include + struct ModuleConfigParameters { bool enable_module{false}; @@ -26,14 +31,56 @@ struct ModuleConfigParameters uint8_t max_module_size{0}; }; +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; + struct BehaviorPathPlannerParameters { bool verbose; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; + ModuleConfigParameters config_dynamic_avoidance; ModuleConfigParameters config_pull_out; - ModuleConfigParameters config_pull_over; + ModuleConfigParameters config_goal_planner; ModuleConfigParameters config_side_shift; ModuleConfigParameters config_lane_change_left; ModuleConfigParameters config_lane_change_right; @@ -45,8 +92,18 @@ struct BehaviorPathPlannerParameters double backward_length_buffer_for_end_of_lane; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; - double minimum_lane_changing_length; + + // common parameters + double min_acc; + double max_acc; + + // lane change parameters + double lane_changing_lateral_jerk{0.5}; + double lateral_acc_switching_velocity{0.4}; + double minimum_lane_changing_velocity{5.6}; + double lane_change_prepare_duration{4.0}; double minimum_prepare_length; + LateralAccelerationMap lane_change_lat_acc_map; double minimum_pull_over_length; double minimum_pull_out_length; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 9c647ba3074b5..6a12c6f7a93db 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include #include #include @@ -54,9 +55,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager( - rclcpp::Node & node, const std::shared_ptr & parameters, - const bool verbose); + PlannerManager(rclcpp::Node & node, const bool verbose); /** * @brief run all candidate and approved modules. @@ -99,12 +98,21 @@ class PlannerManager } /** - * @brief publish all registered modules' debug markers. + * @brief publish all registered modules' markers. */ - void publishDebugMarker() const + void publishMarker() const { std::for_each( - manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishDebugMarker(); }); + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishMarker(); }); + } + + /** + * @brief publish all registered modules' virtual wall. + */ + void publishVirtualWall() const + { + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishVirtualWall(); }); } /** @@ -174,7 +182,7 @@ class PlannerManager const auto result = module_ptr->run(); module_ptr->unlockRTCCommand(); - module_ptr->updateState(); + module_ptr->updateCurrentState(); module_ptr->publishRTCStatus(); @@ -183,6 +191,9 @@ class PlannerManager return result; } + void generateCombinedDrivableArea( + BehaviorModuleOutput & output, const std::shared_ptr & data) const; + /** * @brief get reference path from root_lanelet_ centerline. * @param planner data. @@ -202,11 +213,17 @@ class PlannerManager root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); lanelet::ConstLanelet closest_lane{}; - if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return {}; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + return utils::getReferencePath(closest_lane, data); } - return utils::getReferencePath(closest_lane, parameters_, data); + if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return utils::getReferencePath(closest_lane, data); + } + + return {}; // something wrong. } /** @@ -339,8 +356,6 @@ class PlannerManager boost::optional root_lanelet_{boost::none}; - std::shared_ptr parameters_; - std::vector manager_ptrs_; std::vector approved_module_ptrs_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 895c8c1da49d0..bd65d4a7b285b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -228,12 +228,12 @@ class AvoidanceModule : public SceneModuleInterface // shift point generation: trimmers AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; - void quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const; - void trimSmallShiftLine(AvoidLineArray & shift_lines, const double shift_diff_thres) const; + void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const; void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; void trimMomentaryReturn(AvoidLineArray & shift_lines) const; void trimTooSharpShift(AvoidLineArray & shift_lines) const; - void trimSharpReturn(AvoidLineArray & shift_lines) const; + void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; // shift point generation: return-shift generator void addReturnShiftLineFromEgo( @@ -271,12 +271,25 @@ class AvoidanceModule : public SceneModuleInterface // debug mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; - void setDebugData( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const; - void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; + /** + * @brief fill debug markers. + */ + void updateDebugMarker( + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const; + + /** + * @brief fill information markers that are shown in Rviz by default. + */ + void updateInfoMarker(const AvoidancePlanningData & data) const; + + /** + * @brief fill debug msg that are published as a topic. + */ + void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; + double getLateralMarginFromVelocity(const double velocity) const; double getRSSLongitudinalDistance( @@ -413,13 +426,8 @@ class AvoidanceModule : public SceneModuleInterface double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; - PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const; - PathWithLaneId calcCenterLinePath( - const std::shared_ptr & planner_data, const Pose & pose) const; - // TODO(Horibe): think later. // for unique ID mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp deleted file mode 100644 index e6badb01e716d..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MODULE_HPP_ - -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" - -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using route_handler::Direction; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; - -class AvoidanceByLCModule : public SceneModuleInterface -{ -public: - AvoidanceByLCModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map); - - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - ModuleStatus updateState() override; - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; - CandidateOutput planCandidate() const override; - void processOnEntry() override; - void processOnExit() override; - void updateData() override; - - std::shared_ptr get_debug_msg_array() const; - void acceptVisitor(const std::shared_ptr & visitor) const override; - -#ifndef USE_OLD_ARCHITECTURE - void updateModuleParams(const std::shared_ptr & parameters) - { - parameters_ = parameters; - } -#endif - -private: - std::shared_ptr parameters_; - LaneChangeStatus status_; - PathShifter path_shifter_; - mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneChangeStates current_lane_change_state_; - std::shared_ptr abort_path_; - PathWithLaneId prev_approved_path_; - - double lane_change_lane_length_{200.0}; - double check_distance_{100.0}; - bool is_abort_path_approved_{false}; - bool is_abort_approval_requested_{false}; - bool is_abort_condition_satisfied_{false}; - bool is_activated_ = false; - - void resetParameters(); - - void waitApprovalLeft(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void waitApprovalRight(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void updateRTCStatus(const CandidateOutput & candidate) - { - if (candidate.lateral_shift > 0.0) { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - return; - } - if (candidate.lateral_shift < 0.0) { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - return; - } - - RCLCPP_WARN_STREAM( - getLogger(), - "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); - } - - lanelet::ConstLanelets getCurrentLanes(const PathWithLaneId & path) const - { - const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; - - lanelet::ConstLanelets reference_lanes{}; - for (const auto & p : path.points) { - reference_lanes.push_back( - planner_data_->route_handler->getLaneletsFromId(p.lane_ids.front())); - } - - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(reference_lanes, getEgoPose(), ¤t_lane); - - return route_handler->getLaneletSequence( - current_lane, getEgoPose(), common_parameters.backward_path_length, - common_parameters.forward_path_length); - } - - void removePreviousRTCStatusLeft() - { - if (rtc_interface_ptr_map_.at("left")->isRegistered(uuid_map_.at("left"))) { - rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left")); - } - } - - void removePreviousRTCStatusRight() - { - if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) { - rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right")); - } - } - - AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; - AvoidancePlanningData avoidance_data_; - mutable DebugData debug_data_; - - ObjectDataArray registered_objects_; - mutable ObjectDataArray stopped_objects_; - - ObjectData createObjectData( - const AvoidancePlanningData & data, const PredictedObject & object) const; - - void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; - - lanelet::ConstLanelets get_original_lanes() const; - PathWithLaneId getReferencePath() const; - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; - std::pair getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const; - - void updateLaneChangeStatus(); - void generateExtendedDrivableArea(PathWithLaneId & path); - void updateOutputTurnSignal(BehaviorModuleOutput & output); - void updateSteeringFactorPtr(const BehaviorModuleOutput & output); - bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; - void calcTurnSignalInfo(); - - void updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const; - bool isSafe() const; - bool isValidPath() const; - bool isValidPath(const PathWithLaneId & path) const; - bool isNearEndOfLane() const; - bool isCurrentVelocityLow() const; - bool isAbortConditionSatisfied(); - bool hasFinishedLaneChange() const; - bool isAvoidancePlanRunning() const; - bool isAbortState() const; - - // getter - std_msgs::msg::Header getRouteHeader() const; - void resetPathIfAbort(); - - // debug - void setObjectDebugVisualization() const; - mutable std::unordered_map object_debug_; - mutable std::vector debug_valid_path_; -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MODULE_HPP_ 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 new file mode 100644 index 0000000000000..1dadf8c02ad5e --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -0,0 +1,131 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ + +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +struct DynamicAvoidanceParameters +{ + // obstacle types to avoid + bool avoid_car{true}; + bool avoid_truck{true}; + bool avoid_bus{true}; + bool avoid_trailer{true}; + bool avoid_unknown{false}; + bool avoid_bicycle{false}; + bool avoid_motorcycle{false}; + bool avoid_pedestrian{false}; + double min_obstacle_vel{0.0}; + + // drivable area generation + double lat_offset_from_obstacle{0.0}; + double max_lat_offset_to_avoid{0.0}; + + double max_time_to_collision_overtaking_object{0.0}; + double start_duration_to_avoid_overtaking_object{0.0}; + double end_duration_to_avoid_overtaking_object{0.0}; + double duration_to_hold_avoidance_overtaking_object{0.0}; + + double max_time_to_collision_oncoming_object{0.0}; + double start_duration_to_avoid_oncoming_object{0.0}; + double end_duration_to_avoid_oncoming_object{0.0}; +}; + +class DynamicAvoidanceModule : public SceneModuleInterface +{ +public: + struct DynamicAvoidanceObject + { + explicit DynamicAvoidanceObject( + const PredictedObject & predicted_object, const double arg_path_projected_vel) + : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + path_projected_vel(arg_path_projected_vel), + shape(predicted_object.shape) + { + for (const auto & path : predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + } + + const geometry_msgs::msg::Pose pose; + const double path_projected_vel; + const autoware_auto_perception_msgs::msg::Shape shape; + std::vector predicted_paths{}; + + bool is_left; + }; + +#ifdef USE_OLD_ARCHITECTURE + DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters); +#else + DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters, + const std::unordered_map > & rtc_interface_ptr_map); + + void updateModuleParams(const std::shared_ptr & parameters) + { + parameters_ = parameters; + } +#endif + + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + ModuleStatus updateState() override; + ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + BehaviorModuleOutput plan() override; + CandidateOutput planCandidate() const override; + BehaviorModuleOutput planWaitingApproval() override; + void updateData() override; + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override + { + } + +private: + bool isLabelTargetObstacle(const uint8_t label) const; + std::vector calcTargetObjects() const; + std::pair getAdjacentLanes( + const double forward_distance, const double backward_distance) const; + std::optional calcDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const; + + std::vector target_objects_; + std::shared_ptr parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp similarity index 62% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index 4727275a6d4ec..51fe30c8dc7ee 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/avoidance_by_lc/module.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" #include @@ -29,27 +28,26 @@ namespace behavior_path_planner { -class AvoidanceByLCModuleManager : public SceneModuleManagerInterface +class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface { public: - AvoidanceByLCModuleManager( + DynamicAvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters); std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared( + return std::make_shared( name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr parameters_; - - std::vector> registered_modules_; + std::shared_ptr parameters_; + std::vector> registered_modules_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp similarity index 62% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 799b38166e5a4..5afec90283c0e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" +#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" +#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp" -#include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp" -#include "behavior_path_planner/utils/pull_over/goal_searcher.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" -#include "behavior_path_planner/utils/pull_over/shift_pull_over.hpp" #include #include @@ -56,7 +57,7 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; -enum PathType { +enum class PathType { NONE = 0, SHIFT, ARC_FORWARD, @@ -81,126 +82,186 @@ struct PUllOverStatus bool has_requested_approval{false}; std::optional stop_pose{}; bool is_ready{false}; - bool during_freespace_parking{false}; }; -class PullOverModule : public SceneModuleInterface +class GoalPlannerModule : public SceneModuleInterface { public: #ifdef USE_OLD_ARCHITECTURE - PullOverModule( + GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters); #else - PullOverModule( + GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, + const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::shared_ptr & parameters) { parameters_ = parameters; } #endif BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; bool isExecutionReady() const override; ModuleStatus updateState() override; ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - CandidateOutput planCandidate() const override; void processOnEntry() override; void processOnExit() override; - - void setParameters(const std::shared_ptr & parameters); - + void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } + // not used, but need to override + CandidateOutput planCandidate() const override { return CandidateOutput{}; }; + private: - std::shared_ptr parameters_; + PUllOverStatus status_; + + std::shared_ptr parameters_; + // planner std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; - std::shared_ptr goal_searcher_; - PullOverPath shift_parking_path_; - vehicle_info_util::VehicleInfo vehicle_info_; - - rclcpp::Subscription::SharedPtr occupancy_grid_sub_; + std::unique_ptr fixed_goal_planner_; - PUllOverStatus status_; - std::shared_ptr occupancy_grid_map_; + // goal searcher + std::shared_ptr goal_searcher_; std::optional modified_goal_pose_; std::optional prev_goal_id_; Pose refined_goal_pose_; GoalCandidates goal_candidates_; + + // collision detector + // need to be shared_ptr to be used in planner and goal searcher + std::shared_ptr occupancy_grid_map_; + + // pull over path std::vector pull_over_path_candidates_; std::optional closest_start_pose_; - GeometricParallelParking parallel_parking_planner_; - ParallelParkingParameters parallel_parking_parameters_; + + // check stopped and stuck state std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; tier4_autoware_utils::LinearRing2d vehicle_footprint_; + + // save last time and pose std::unique_ptr last_received_time_; std::unique_ptr last_approved_time_; std::unique_ptr last_increment_time_; + std::unique_ptr last_path_update_time_; std::unique_ptr last_approved_pose_; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. - double approximate_pull_over_distance_ = 20.0; + double approximate_pull_over_distance_{20.0}; - bool incrementPathIndex(); - PathWithLaneId getCurrentPath() const; + // for parking policy + bool left_side_parking_{true}; + mutable bool allow_goal_modification_{false}; // need to be set in isExecutionRequested + + // pre-generate lane parking paths in a separate thread + rclcpp::TimerBase::SharedPtr lane_parking_timer_; + rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; + std::mutex mutex_; + + // generate freespace parking paths in a separate thread + rclcpp::TimerBase::SharedPtr freespace_parking_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; + + // collision check + void initializeOccupancyGridMap(); + void updateOccupancyGrid(); + bool checkCollision(const PathWithLaneId & path) const; + + // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; - ParallelParkingParameters getGeometricPullOverParameters() const; - double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; + + // stop or decelerate void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart(const Pose & search_start_pose, PathWithLaneId & path) const; - std::pair calcDistanceToPathChange() const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); boost::optional calcFeasibleDecelDistance(const double target_velocity) const; - double calcModuleRequestLength() const; void keepStoppedWithCurrentPath(PathWithLaneId & path); + double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; + // status bool isStopped(); bool isStopped( std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); - bool hasFinishedPullOver(); - void updateOccupancyGrid(); + bool hasFinishedGoalPlanner(); + bool isOnGoal() const; + double calcModuleRequestLength() const; void resetStatus(); + bool needPathUpdate(const double path_update_duration) const; + bool isStuck(); + bool hasDecidedPath() const; + void decideVelocity(); - bool checkCollision(const PathWithLaneId & path) const; + // validation bool hasEnoughDistance(const PullOverPath & pull_over_path) const; + bool isCrossingPossible( + const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const; + bool isCrossingPossible( + const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const; + bool isCrossingPossible(const PullOverPath & pull_over_path) const; + bool hasEnoughTimePassedSincePathUpdate(const double duration) const; - TurnSignalInfo calcTurnSignalInfo() const; - + // freespace parking bool planFreespacePath(); - bool returnToLaneParking(); - bool isStuck(); + void returnToLaneParking(); + + // plan pull over path + BehaviorModuleOutput planWithGoalModification(); + BehaviorModuleOutput planWaitingApprovalWithGoalModification(); + void selectSafePullOverPath(); + + // deal with pull over partial paths + PathWithLaneId getCurrentPath() const; + bool incrementPathIndex(); + void transitionToNextPathIfFinishingCurrentPath(); + + // lanes and drivable area + void setLanes(); + void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - // timer for generating pull over path candidates + // output setter + void setOutput(BehaviorModuleOutput & output); + void setStopPath(BehaviorModuleOutput & output); + void setModifiedGoal(BehaviorModuleOutput & output) const; + void setTurnSignalInfo(BehaviorModuleOutput & output) const; + + // new turn signal + TurnSignalInfo calcTurnSignalInfo() const; + + // timer for generating pull over path candidates in a separate thread void onTimer(); - rclcpp::TimerBase::SharedPtr lane_parking_timer_; - rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; - std::mutex mutex_; + void onFreespaceParkingTimer(); + + // flag for the interface which do not support `allow_goal_modification` + // when the goal is in `road_shoulder`, always allow goal modification. + bool checkOriginalGoalIsInShoulder() const; + + // steering factor + void updateSteeringFactor( + const std::array & pose, const std::array distance, const uint16_t type); + + // rtc + std::pair calcDistanceToPathChange() const; // debug void setDebugData(); void printParkingPositionError() const; - - void onFreespaceParkingTimer(); - rclcpp::TimerBase::SharedPtr freespace_parking_timer_; - rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp similarity index 62% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index fbad4d6df736b..31920ecd640f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" +#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include @@ -27,26 +27,26 @@ namespace behavior_path_planner { -class PullOverModuleManager : public SceneModuleManagerInterface +class GoalPlannerModuleManager : public SceneModuleManagerInterface { public: - PullOverModuleManager( + GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters); std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr parameters_; + std::shared_ptr parameters_; - std::vector> registered_modules_; + std::vector> registered_modules_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp new file mode 100644 index 0000000000000..e58a8c07850cf --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/normal.hpp" + +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::LaneChangeDebugMsg; +using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; +using AvoidanceDebugData = DebugData; + +class AvoidanceByLaneChange : public NormalLaneChange +{ +public: + AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + void updateSpecialData() override; + + std::pair getSafePath(LaneChangePath & safe_path) const override; + +private: + std::shared_ptr avoidance_parameters_; + std::shared_ptr avoidance_by_lane_change_parameters_; + + AvoidancePlanningData calcAvoidancePlanningData(AvoidanceDebugData & debug) const; + AvoidancePlanningData avoidance_data_; + mutable AvoidanceDebugData avoidance_debug_data_; + + ObjectDataArray registered_objects_; + mutable ObjectDataArray stopped_objects_; + + ObjectData createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const; + + void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index af119943cb21c..827eddbad499d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -22,6 +22,7 @@ #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include #include #include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" @@ -30,8 +31,6 @@ #include #include -#include - #include #include #include @@ -41,6 +40,7 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -52,73 +52,94 @@ using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; class LaneChangeBase { public: - LaneChangeBase(const std::shared_ptr & parameters, Direction direction) - : parameters_{parameters}, direction_{direction} + LaneChangeBase( + std::shared_ptr parameters, LaneChangeModuleType type, + Direction direction) + : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} { } + LaneChangeBase(const LaneChangeBase &) = delete; + LaneChangeBase(LaneChangeBase &&) = delete; + LaneChangeBase & operator=(const LaneChangeBase &) = delete; + LaneChangeBase & operator=(LaneChangeBase &&) = delete; virtual ~LaneChangeBase() = default; - virtual void updateLaneChangeStatus( - const PathWithLaneId & prev_module_reference_path, - const PathWithLaneId & previous_module_path) = 0; + virtual void updateLaneChangeStatus() = 0; + + virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; + + virtual BehaviorModuleOutput generateOutput() = 0; + + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; - virtual std::pair getSafePath( - const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path, - LaneChangePath & safe_path) const = 0; + virtual PathWithLaneId getReferencePath() const = 0; - virtual PathWithLaneId generatePlannedPath( - const std::vector & prev_drivable_lanes) = 0; + virtual void resetParameters() = 0; - virtual void generateExtendedDrivableArea( - const std::vector & prev_drivable_lanes, PathWithLaneId & path) = 0; + virtual TurnSignalInfo updateOutputTurnSignal() = 0; virtual bool hasFinishedLaneChange() const = 0; - virtual PathWithLaneId getReferencePath() const = 0; + virtual bool hasFinishedAbort() const = 0; - virtual bool isCancelConditionSatisfied() = 0; + virtual bool isAbortState() const = 0; - virtual bool isAbortConditionSatisfied(const Pose & pose) = 0; + virtual bool isAbleToReturnCurrentLane() const = 0; - virtual void resetParameters() = 0; + virtual LaneChangePath getLaneChangePath() const = 0; - virtual TurnSignalInfo updateOutputTurnSignal() = 0; + virtual bool isEgoOnPreparePhase() const = 0; - const LaneChangeStatus & getLaneChangeStatus() const { return status_; } + virtual bool isRequiredStop(const bool is_object_coming_from_rear) const = 0; + + virtual PathSafetyStatus isApprovedPathSafe() const = 0; + + virtual bool isNearEndOfLane() const = 0; + + virtual bool getAbortPath() = 0; + + virtual void setPreviousModulePaths( + const std::shared_ptr & prev_module_reference_path, + const std::shared_ptr & prev_module_path) + { + if (prev_module_reference_path) { + prev_module_reference_path_ = *prev_module_reference_path; + } + if (prev_module_path) { + prev_module_path_ = *prev_module_path; + } + }; + + virtual void setPreviousDrivableAreaInfo(const DrivableAreaInfo & prev_drivable_area_info) + { + prev_drivable_area_info_ = prev_drivable_area_info; + } - LaneChangePath getLaneChangePath() const + virtual void setPreviousTurnSignalInfo(const TurnSignalInfo & prev_turn_signal_info) { - return isAbortState() ? *abort_path_ : status_.lane_change_path; + prev_turn_signal_info_ = prev_turn_signal_info; } + virtual void updateSpecialData() {} + + const LaneChangeStatus & getLaneChangeStatus() const { return status_; } + const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; } const CollisionCheckDebugMap & getDebugData() const { return object_debug_; } - bool isAbortState() const - { - if (!parameters_->enable_abort_lane_change) { - return false; - } + const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); + const Point & getEgoPosition() const { return getEgoPose().position; } - if (!is_within_current_lane) { - return false; - } + const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } - if (current_lane_change_state_ != LaneChangeStates::Abort) { - return false; - } + const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } - if (!abort_path_) { - return false; - } + bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; } - return true; - } + bool isAbortEnabled() const { return lane_change_parameters_->enable_abort_lane_change; } bool isSafe() const { return status_.is_safe; } @@ -126,64 +147,75 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - std_msgs::msg::Header getRouteHeader() const - { - return planner_data_->route_handler->getRouteHeader(); - } - void setData(const std::shared_ptr & data) { planner_data_ = data; } - const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } + void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } - const Point & getEgoPosition() const { return getEgoPose().position; } + void toStopState() { current_lane_change_state_ = LaneChangeStates::Stop; } - const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } + void toCancelState() { current_lane_change_state_ = LaneChangeStates::Cancel; } - std::shared_ptr getRouteHandler() const { return planner_data_->route_handler; } + void toAbortState() { current_lane_change_state_ = LaneChangeStates::Abort; } double getEgoVelocity() const { return getEgoTwist().linear.x; } - const Direction & getDirection() const { return direction_; } + std::shared_ptr getRouteHandler() const { return planner_data_->route_handler; } + + std_msgs::msg::Header getRouteHeader() const { return getRouteHandler()->getRouteHeader(); } + + std::string getModuleTypeStr() const { return std::string{magic_enum::enum_name(type_)}; } + + Direction getDirection() const + { + if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) { + const auto lateral_shift = utils::lane_change::getLateralShift(status_.lane_change_path); + return lateral_shift > 0.0 ? Direction::LEFT : Direction::RIGHT; + } + + return direction_; + } protected: - virtual lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const = 0; + virtual lanelet::ConstLanelets getCurrentLanes() const = 0; + + virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; + + virtual PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, const double current_velocity, + const double prepare_velocity) const = 0; + + virtual bool getLaneChangePaths( + const lanelet::ConstLanelets & original_lanelets, + const lanelet::ConstLanelets & target_lanelets, Direction direction, + LaneChangePaths * candidate_paths) const = 0; - virtual bool isApprovedPathSafe(Pose & ego_pose_before_collision) const = 0; + virtual std::vector getDrivableLanes() const = 0; virtual void calcTurnSignalInfo() = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; - bool isNearEndOfLane() const - { - const auto & current_pose = getEgoPose(); - const double threshold = utils::calcTotalLaneChangeLength(planner_data_->parameters); - - return (std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) - - threshold) < planner_data_->parameters.backward_length_buffer_for_end_of_lane; - } + virtual bool isAbleToStopSafely() const = 0; - bool isCurrentSpeedLow() const - { - constexpr double threshold_ms = 10.0 * 1000 / 3600; - return getEgoTwist().linear.x < threshold_ms; - } + virtual lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; LaneChangeStatus status_{}; PathShifter path_shifter_{}; LaneChangeStates current_lane_change_state_{}; - std::shared_ptr parameters_{}; + std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; + PathWithLaneId prev_module_reference_path_{}; + PathWithLaneId prev_module_path_{}; + DrivableAreaInfo prev_drivable_area_info_{}; + TurnSignalInfo prev_turn_signal_info_{}; PathWithLaneId prev_approved_path_{}; - double lane_change_lane_length_{200.0}; - double check_distance_{100.0}; - bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; bool is_activated_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp index c7e8c3b5237fa..51a71ef7cd061 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp @@ -27,11 +27,24 @@ class ExternalRequestLaneChange : public NormalLaneChange ExternalRequestLaneChange( const std::shared_ptr & parameters, Direction direction); + ExternalRequestLaneChange(const ExternalRequestLaneChange &) = delete; + ExternalRequestLaneChange(ExternalRequestLaneChange &&) = delete; + ExternalRequestLaneChange & operator=(const ExternalRequestLaneChange &) = delete; + ExternalRequestLaneChange & operator=(ExternalRequestLaneChange &&) = delete; ~ExternalRequestLaneChange() override = default; +}; + +class ExternalRequestLaneChangeBT : public NormalLaneChangeBT +{ +public: + ExternalRequestLaneChangeBT( + const std::shared_ptr & parameters, Direction direction); -protected: - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const override; + ExternalRequestLaneChangeBT(const ExternalRequestLaneChangeBT &) = delete; + ExternalRequestLaneChangeBT(ExternalRequestLaneChangeBT &&) = delete; + ExternalRequestLaneChangeBT & operator=(const ExternalRequestLaneChangeBT &) = delete; + ExternalRequestLaneChangeBT & operator=(ExternalRequestLaneChangeBT &&) = delete; + ~ExternalRequestLaneChangeBT() override = default; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp deleted file mode 100644 index 157aeb9352bd8..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ - -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" - -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; - -#ifdef USE_OLD_ARCHITECTURE - -class ExternalRequestLaneChangeModule : public SceneModuleInterface -{ -public: - enum class Direction { - RIGHT = 0, - LEFT, - }; - - ExternalRequestLaneChangeModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const Direction & direction); - - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - BT::NodeStatus updateState() override; - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; - CandidateOutput planCandidate() const override; - void processOnEntry() override; - void processOnExit() override; - - std::shared_ptr get_debug_msg_array() const; - void acceptVisitor( - [[maybe_unused]] const std::shared_ptr & visitor) const override; - -protected: - std::shared_ptr parameters_; - LaneChangeStatus status_; - PathShifter path_shifter_; - mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneChangeStates current_lane_change_state_; - std::shared_ptr abort_path_; - PathWithLaneId prev_approved_path_; - - Direction direction_; - - double lane_change_lane_length_{200.0}; - double check_distance_{100.0}; - bool is_abort_path_approved_{false}; - bool is_abort_approval_requested_{false}; - bool is_abort_condition_satisfied_{false}; - bool is_activated_ = false; - - void resetParameters(); - void resetPathIfAbort(); - - void waitApproval(const double start_distance, const double finish_distance) - { - updateRTCStatus(start_distance, finish_distance); - is_waiting_approval_ = true; - } - - lanelet::ConstLanelets get_original_lanes() const; - PathWithLaneId getReferencePath() const; - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; - std::pair getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const; - - void updateLaneChangeStatus(); - void generateExtendedDrivableArea(PathWithLaneId & path); - void updateOutputTurnSignal(BehaviorModuleOutput & output); - void updateSteeringFactorPtr(const BehaviorModuleOutput & output); - void updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const; - void calcTurnSignalInfo(); - - bool isSafe() const; - bool isValidPath(const PathWithLaneId & path) const; - bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; - bool isNearEndOfLane() const; - bool isCurrentVelocityLow() const; - bool isAbortConditionSatisfied(); - bool hasFinishedLaneChange() const; - bool isAbortState() const; - std_msgs::msg::Header getRouteHeader() const; - - // debug - mutable std::unordered_map object_debug_; - mutable std::vector debug_valid_path_; - - void setObjectDebugVisualization() const; -}; - -class ExternalRequestLaneChangeRightModule : public ExternalRequestLaneChangeModule -{ -public: - ExternalRequestLaneChangeRightModule( - const std::string & name, rclcpp::Node & node, - std::shared_ptr parameters); -}; - -class ExternalRequestLaneChangeLeftModule : public ExternalRequestLaneChangeModule -{ -public: - ExternalRequestLaneChangeLeftModule( - const std::string & name, rclcpp::Node & node, - std::shared_ptr parameters); -}; -#endif - -} // namespace behavior_path_planner -// clang-format off -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ // NOLINT -// clang-format on diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 07ad05488bee5..f529d04a9010d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #include "behavior_path_planner/marker_util/lane_change/debug.hpp" +#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" @@ -54,49 +55,136 @@ class LaneChangeInterface : public SceneModuleInterface { public: LaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map > & rtc_interface_ptr_map, std::unique_ptr && module_type); + LaneChangeInterface(const LaneChangeInterface &) = delete; + LaneChangeInterface(LaneChangeInterface &&) = delete; + LaneChangeInterface & operator=(const LaneChangeInterface &) = delete; + LaneChangeInterface & operator=(LaneChangeInterface &&) = delete; + ~LaneChangeInterface() override = default; + void processOnEntry() override; + void processOnExit() override; bool isExecutionRequested() const override; + bool isExecutionReady() const override; ModuleStatus updateState() override; + void updateData() override; + BehaviorModuleOutput plan() override; + BehaviorModuleOutput planWaitingApproval() override; + CandidateOutput planCandidate() const override; std::shared_ptr get_debug_msg_array() const; void acceptVisitor(const std::shared_ptr & visitor) const override; + void updateModuleParams(const std::shared_ptr & parameters); void setData(const std::shared_ptr & data) override; -private: +protected: std::shared_ptr parameters_; + std::unique_ptr module_type_; void resetPathIfAbort(); -protected: void setObjectDebugVisualization() const; + void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; + mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; std::unique_ptr prev_approved_path_; + void clearAbortApproval() { is_abort_path_approved_ = false; } + bool is_abort_path_approved_{false}; + bool is_abort_approval_requested_{false}; }; + +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map > & rtc_interface_ptr_map); + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; + +class LaneChangeBTInterface : public LaneChangeInterface +{ +public: + LaneChangeBTInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map > & rtc_interface_ptr_map, + std::unique_ptr && module_type); + + LaneChangeBTInterface(const LaneChangeBTInterface &) = delete; + LaneChangeBTInterface(LaneChangeBTInterface &&) = delete; + LaneChangeBTInterface & operator=(const LaneChangeBTInterface &) = delete; + LaneChangeBTInterface & operator=(LaneChangeBTInterface &&) = delete; + ~LaneChangeBTInterface() override = default; + + void processOnEntry() override; + + BehaviorModuleOutput plan() override; + + BehaviorModuleOutput planWaitingApproval() override; + + CandidateOutput planCandidate() const override; + + void acceptVisitor(const std::shared_ptr & visitor) const override; + +protected: + bool is_activated_{false}; +}; + +class LaneChangeBTModule : public LaneChangeBTInterface +{ +public: + LaneChangeBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters); + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; + +class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface +{ +public: + ExternalRequestLaneChangeLeftBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters); +}; + +class ExternalRequestLaneChangeRightBTModule : public LaneChangeBTInterface +{ +public: + ExternalRequestLaneChangeRightBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters); +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp deleted file mode 100644 index 27a64e2e1d585..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ /dev/null @@ -1,202 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ - -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" - -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using route_handler::Direction; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; - -class LaneChangeModule : public SceneModuleInterface -{ -public: -#ifdef USE_OLD_ARCHITECTURE - LaneChangeModule( - const std::string & name, rclcpp::Node & node, - std::shared_ptr parameters); -#else - LaneChangeModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map, - Direction direction, LaneChangeModuleType type); -#endif - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - ModuleStatus updateState() override; - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; - CandidateOutput planCandidate() const override; - void processOnEntry() override; - void processOnExit() override; - - std::shared_ptr get_debug_msg_array() const; - void acceptVisitor( - [[maybe_unused]] const std::shared_ptr & visitor) const override; - -#ifndef USE_OLD_ARCHITECTURE - void updateModuleParams(const std::shared_ptr & parameters) - { - parameters_ = parameters; - } -#endif - -private: - std::shared_ptr parameters_; - LaneChangeStatus status_; - PathShifter path_shifter_; - mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneChangeStates current_lane_change_state_; - std::shared_ptr abort_path_; - PathWithLaneId prev_approved_path_; - - double lane_change_lane_length_{200.0}; - double check_distance_{100.0}; - bool is_abort_path_approved_{false}; - bool is_abort_approval_requested_{false}; - bool is_abort_condition_satisfied_{false}; - bool is_activated_{false}; - -#ifdef USE_OLD_ARCHITECTURE - UUID candidate_uuid_; -#else - Direction direction_{Direction::NONE}; - LaneChangeModuleType type_{LaneChangeModuleType::NORMAL}; -#endif - - void resetParameters(); - -#ifdef USE_OLD_ARCHITECTURE - void waitApprovalLeft(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void waitApprovalRight(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void updateRTCStatus(const CandidateOutput & candidate) - { - if (candidate.lateral_shift > 0.0) { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_map_.at("left"); - return; - } - if (candidate.lateral_shift < 0.0) { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_map_.at("right"); - return; - } - - RCLCPP_WARN_STREAM( - getLogger(), - "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); - } - - void removePreviousRTCStatusLeft() - { - if (rtc_interface_ptr_map_.at("left")->isRegistered(uuid_map_.at("left"))) { - rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left")); - } - } - - void removePreviousRTCStatusRight() - { - if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) { - rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right")); - } - } -#endif - - PathWithLaneId getReferencePath() const; -#ifdef USE_OLD_ARCHITECTURE - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; -#endif - std::pair getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const; - PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const; - - void updateLaneChangeStatus(); - void generateExtendedDrivableArea(PathWithLaneId & path); - void updateOutputTurnSignal(BehaviorModuleOutput & output); - void updateSteeringFactorPtr(const BehaviorModuleOutput & output); - bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; - void calcTurnSignalInfo(); - - void updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const; - bool isSafe() const; - bool isValidPath() const; - bool isValidPath(const PathWithLaneId & path) const; - bool isNearEndOfLane() const; - bool isCurrentVelocityLow() const; - bool isAbortConditionSatisfied(); - bool hasFinishedLaneChange() const; - bool isAbortState() const; - - // getter - std_msgs::msg::Header getRouteHeader() const; - void resetPathIfAbort(); - - // debug - void setObjectDebugVisualization() const; - mutable std::unordered_map object_debug_; - mutable std::vector debug_valid_path_; -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 39452d6367fae..ebee8f3d3a07a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -42,7 +42,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; -private: +protected: std::shared_ptr parameters_; std::vector> registered_modules_; @@ -52,6 +52,23 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface LaneChangeModuleType type_; }; +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + std::shared_ptr createNewSceneModuleInstance() override; + + // void updateModuleParams(const std::vector & parameters) override; + +private: + std::shared_ptr avoidance_parameters_; + std::shared_ptr avoidance_by_lane_change_parameters_; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 8320d66e16a57..9e262a677d365 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -35,45 +35,103 @@ using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; class NormalLaneChange : public LaneChangeBase { public: - NormalLaneChange(const std::shared_ptr & parameters, Direction direction); - + NormalLaneChange( + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction); + + NormalLaneChange(const NormalLaneChange &) = delete; + NormalLaneChange(NormalLaneChange &&) = delete; + NormalLaneChange & operator=(const NormalLaneChange &) = delete; + NormalLaneChange & operator=(NormalLaneChange &&) = delete; ~NormalLaneChange() override = default; - void updateLaneChangeStatus( - const PathWithLaneId & prev_module_reference_path, - const PathWithLaneId & previous_module_path) override; + void updateLaneChangeStatus() override; - std::pair getSafePath( - const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path, - LaneChangePath & safe_path) const override; + std::pair getSafePath(LaneChangePath & safe_path) const override; - PathWithLaneId generatePlannedPath( - const std::vector & prev_drivable_lanes) override; + LaneChangePath getLaneChangePath() const override; - void generateExtendedDrivableArea( - const std::vector & prev_drivable_lanes, PathWithLaneId & path) override; + BehaviorModuleOutput generateOutput() override; - bool hasFinishedLaneChange() const override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) override; PathWithLaneId getReferencePath() const override; - bool isCancelConditionSatisfied() override; - - bool isAbortConditionSatisfied(const Pose & pose) override; - void resetParameters() override; TurnSignalInfo updateOutputTurnSignal() override; + bool getAbortPath() override; + + PathSafetyStatus isApprovedPathSafe() const override; + + bool isRequiredStop(const bool is_object_coming_from_rear) const override; + + bool isNearEndOfLane() const override; + + bool hasFinishedLaneChange() const override; + + bool isAbleToReturnCurrentLane() const override; + + bool isEgoOnPreparePhase() const override; + + bool isAbleToStopSafely() const override; + + bool hasFinishedAbort() const override; + + bool isAbortState() const override; + protected: + lanelet::ConstLanelets getCurrentLanes() const override; + lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const override; + const lanelet::ConstLanelets & current_lanes, Direction direction) const override; - bool isApprovedPathSafe(Pose & ego_pose_before_collision) const override; + int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + + PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, const double current_velocity, + const double prepare_velocity) const override; + + bool getLaneChangePaths( + const lanelet::ConstLanelets & original_lanelets, + const lanelet::ConstLanelets & target_lanelets, Direction direction, + LaneChangePaths * candidate_paths) const override; + + std::vector getDrivableLanes() const override; void calcTurnSignalInfo() override; bool isValidPath(const PathWithLaneId & path) const override; }; + +class NormalLaneChangeBT : public NormalLaneChange +{ +public: + NormalLaneChangeBT( + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction); + + NormalLaneChangeBT(const NormalLaneChangeBT &) = delete; + NormalLaneChangeBT(NormalLaneChangeBT &&) = delete; + NormalLaneChangeBT & operator=(const NormalLaneChangeBT &) = delete; + NormalLaneChangeBT & operator=(NormalLaneChangeBT &&) = delete; + ~NormalLaneChangeBT() override = default; + + PathWithLaneId getReferencePath() const override; + +protected: + lanelet::ConstLanelets getCurrentLanes() const override; + + int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + + PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, const double current_velocity, + const double prepare_velocity) const override; + + std::vector getDrivableLanes() const override; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp index 020f263c564af..d908dea155180 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp @@ -33,9 +33,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; class LaneFollowingModule : public SceneModuleInterface { public: - LaneFollowingModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); + LaneFollowingModule(const std::string & name, rclcpp::Node & node); bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -52,8 +50,6 @@ class LaneFollowingModule : public SceneModuleInterface } private: - std::shared_ptr parameters_; - BehaviorModuleOutput getReferencePath() const; void initParam(); }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 78cc37b0a9d68..63720996782f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -89,7 +89,6 @@ class PullOutModule : public SceneModuleInterface BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; - void processOnEntry() override; void processOnExit() override; void setParameters(const std::shared_ptr & parameters) @@ -118,7 +117,6 @@ class PullOutModule : public SceneModuleInterface std::shared_ptr getCurrentPlanner() const; PathWithLaneId getFullPath() const; - ParallelParkingParameters getGeometricPullOutParameters() const; std::vector searchPullOutStartPoses(); std::shared_ptr lane_departure_checker_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 4ec88e4024be3..85b2f80cb2093 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -44,8 +44,13 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using motion_utils::createDeadLineVirtualWallMarker; +using motion_utils::createSlowDownVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using unique_identifier_msgs::msg::UUID; @@ -63,14 +68,30 @@ class SceneModuleInterface clock_{node.get_clock()}, is_waiting_approval_{false}, is_locked_new_module_launch_{false}, +#ifdef USE_OLD_ARCHITECTURE current_state_{ModuleStatus::SUCCESS}, +#else + current_state_{ModuleStatus::IDLE}, +#endif rtc_interface_ptr_map_(rtc_interface_ptr_map), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) { #ifdef USE_OLD_ARCHITECTURE - const auto ns = std::string("~/debug/") + utils::convertToSnakeCase(name); - pub_debug_marker_ = node.create_publisher(ns, 20); + { + const auto ns = std::string("~/debug/") + utils::convertToSnakeCase(name); + pub_debug_marker_ = node.create_publisher(ns, 20); + } + + { + const auto ns = std::string("~/info/") + utils::convertToSnakeCase(name); + pub_info_marker_ = node.create_publisher(ns, 20); + } + + { + const auto ns = std::string("~/virtual_wall/") + utils::convertToSnakeCase(name); + pub_virtual_wall_ = node.create_publisher(ns, 20); + } #endif for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -87,6 +108,11 @@ class SceneModuleInterface */ virtual ModuleStatus updateState() = 0; + /** + * @brief Set the current_state_ based on updateState output. + */ + virtual void updateCurrentState() { current_state_ = updateState(); } + /** * @brief If the module plan customized reference path while waiting approval, it should output * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. @@ -118,6 +144,13 @@ class SceneModuleInterface out.path = utils::generateCenterLinePath(planner_data_); const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); + + // for new architecture + const auto lanes = utils::getLaneletsFromPath(*out.path, planner_data_->route_handler); + const auto drivable_lanes = utils::generateDrivableLanes(lanes); + out.drivable_area_info.drivable_lanes = + getNonOverlappingExpandedLanes(*out.path, drivable_lanes); + return out; } @@ -206,10 +239,15 @@ class SceneModuleInterface } /** - * @brief Return true if the activation command is received + * @brief Return true if the activation command is received from the RTC interface. + * If no RTC interface is registered, return true. */ bool isActivated() { + if (rtc_interface_ptr_map_.empty()) { + return true; + } + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { if (itr->second->isRegistered(uuid_map_.at(itr->first))) { return itr->second->isActivated(uuid_map_.at(itr->first)); @@ -258,7 +296,39 @@ class SceneModuleInterface virtual void setData(const std::shared_ptr & data) { planner_data_ = data; } #ifdef USE_OLD_ARCHITECTURE + void publishInfoMarker() { pub_info_marker_->publish(info_marker_); } + void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } + + void publishVirtualWall() + { + MarkerArray markers{}; + + const auto opt_stop_pose = getStopPose(); + if (!!opt_stop_pose) { + const auto virtual_wall = createStopVirtualWallMarker( + opt_stop_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_slow_pose = getSlowPose(); + if (!!opt_slow_pose) { + const auto virtual_wall = createSlowDownVirtualWallMarker( + opt_slow_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_dead_pose = getDeadPose(); + if (!!opt_dead_pose) { + const auto virtual_wall = createDeadLineVirtualWallMarker( + opt_dead_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); + appendMarkerArray(virtual_wall, &markers); + } + + pub_virtual_wall_->publish(markers); + + resetWallPoses(); + } #endif bool isWaitingApproval() const { return is_waiting_approval_; } @@ -273,7 +343,9 @@ class SceneModuleInterface PlanResult getPathReference() const { return path_reference_; } - MarkerArray getDebugMarkers() { return debug_marker_; } + MarkerArray getInfoMarkers() const { return info_marker_; } + + MarkerArray getDebugMarkers() const { return debug_marker_; } ModuleStatus getCurrentStatus() const { return current_state_; } @@ -281,15 +353,74 @@ class SceneModuleInterface std::string name() const { return name_; } + boost::optional getStopPose() const + { + if (!stop_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(stop_pose_.get(), base_link2front, 0.0, 0.0); + } + + boost::optional getSlowPose() const + { + if (!slow_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(slow_pose_.get(), base_link2front, 0.0, 0.0); + } + + boost::optional getDeadPose() const + { + if (!dead_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(dead_pose_.get(), base_link2front, 0.0, 0.0); + } + + void resetWallPoses() + { + stop_pose_ = boost::none; + slow_pose_ = boost::none; + dead_pose_ = boost::none; + } + rclcpp::Logger getLogger() const { return logger_; } + void setIsSimultaneousExecutableAsApprovedModule(const bool enable) + { + is_simultaneously_executable_as_approved_module_ = enable; + } + + bool isSimultaneousExecutableAsApprovedModule() const + { + return is_simultaneously_executable_as_approved_module_; + } + + void setIsSimultaneousExecutableAsCandidateModule(const bool enable) + { + is_simultaneously_executable_as_candidate_module_ = enable; + } + + bool isSimultaneousExecutableAsCandidateModule() const + { + return is_simultaneously_executable_as_candidate_module_; + } + private: std::string name_; rclcpp::Logger logger_; #ifdef USE_OLD_ARCHITECTURE + rclcpp::Publisher::SharedPtr pub_info_marker_; rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; #endif BehaviorModuleOutput previous_module_output_; @@ -303,14 +434,14 @@ class SceneModuleInterface for (const auto & rtc_type : rtc_types) { const auto snake_case_name = utils::convertToSnakeCase(name); const auto rtc_interface_name = - rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; + rtc_type.empty() ? snake_case_name : (snake_case_name + "_" + rtc_type); rtc_interface_ptr_map.emplace( rtc_type, std::make_shared(&node, rtc_interface_name)); } return rtc_interface_ptr_map; } - void updateRTCStatus(const double start_distance, const double finish_distance) + virtual void updateRTCStatus(const double start_distance, const double finish_distance) { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { if (itr->second) { @@ -354,6 +485,21 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } + std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes) const + { + const auto & dp = planner_data_->drivable_area_expansion_parameters; + + const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + return expanded_lanes; + } + + bool is_simultaneously_executable_as_approved_module_{false}; + bool is_simultaneously_executable_as_candidate_module_{false}; + rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; @@ -366,12 +512,24 @@ class SceneModuleInterface PlanResult path_candidate_; PlanResult path_reference_; - ModuleStatus current_state_; +#ifdef USE_OLD_ARCHITECTURE + ModuleStatus current_state_{ModuleStatus::SUCCESS}; +#else + ModuleStatus current_state_{ModuleStatus::IDLE}; +#endif std::unordered_map> rtc_interface_ptr_map_; std::unique_ptr steering_factor_interface_ptr_; + mutable boost::optional stop_pose_{boost::none}; + + mutable boost::optional slow_pose_{boost::none}; + + mutable boost::optional dead_pose_{boost::none}; + + mutable MarkerArray info_marker_; + mutable MarkerArray debug_marker_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index d0dfb84d674f6..6e26fbfac2d06 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -32,6 +32,9 @@ namespace behavior_path_planner { +using motion_utils::createDeadLineVirtualWallMarker; +using motion_utils::createSlowDownVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; @@ -61,19 +64,22 @@ class SceneModuleManagerInterface rtc_type, std::make_shared(node, rtc_interface_name)); } + pub_info_marker_ = node->create_publisher("~/info/" + name, 20); pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); + pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name, 20); } virtual ~SceneModuleManagerInterface() = default; SceneModulePtr getNewModule() { - if (idling_module_ != nullptr) { - return idling_module_; + if (idling_module_ptr_ != nullptr) { + idling_module_ptr_->onEntry(); + return idling_module_ptr_; } - idling_module_ = createNewSceneModuleInstance(); - return idling_module_; + idling_module_ptr_ = createNewSceneModuleInstance(); + return idling_module_ptr_; } bool isExecutionRequested( @@ -89,6 +95,10 @@ class SceneModuleManagerInterface void registerNewModule( const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) { + module_ptr->setIsSimultaneousExecutableAsApprovedModule( + enable_simultaneous_execution_as_approved_module_); + module_ptr->setIsSimultaneousExecutableAsCandidateModule( + enable_simultaneous_execution_as_candidate_module_); module_ptr->setData(planner_data_); module_ptr->setPreviousModuleOutput(previous_module_output); module_ptr->onEntry(); @@ -108,11 +118,12 @@ class SceneModuleManagerInterface } module_ptr.reset(); + idling_module_ptr_.reset(); pub_debug_marker_->publish(MarkerArray{}); } - void publishDebugMarker() const + void publishVirtualWall() const { using tier4_autoware_utils::appendMarkerArray; @@ -122,18 +133,64 @@ class SceneModuleManagerInterface uint32_t marker_id = marker_offset; for (const auto & m : registered_modules_) { + const auto opt_stop_pose = m->getStopPose(); + if (!!opt_stop_pose) { + const auto virtual_wall = createStopVirtualWallMarker( + opt_stop_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_slow_pose = m->getSlowPose(); + if (!!opt_slow_pose) { + const auto virtual_wall = createSlowDownVirtualWallMarker( + opt_slow_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_dead_pose = m->getDeadPose(); + if (!!opt_dead_pose) { + const auto virtual_wall = createDeadLineVirtualWallMarker( + opt_dead_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + appendMarkerArray(virtual_wall, &markers); + } + + m->resetWallPoses(); + } + + pub_virtual_wall_->publish(markers); + } + + void publishMarker() const + { + using tier4_autoware_utils::appendMarkerArray; + + MarkerArray info_markers{}; + MarkerArray debug_markers{}; + + const auto marker_offset = std::numeric_limits::max(); + + uint32_t marker_id = marker_offset; + for (const auto & m : registered_modules_) { + for (auto & marker : m->getInfoMarkers().markers) { + marker.id += marker_id; + info_markers.markers.push_back(marker); + } + for (auto & marker : m->getDebugMarkers().markers) { marker.id += marker_id; - markers.markers.push_back(marker); + debug_markers.markers.push_back(marker); } + marker_id += marker_offset; } - if (registered_modules_.empty() && idling_module_ != nullptr) { - appendMarkerArray(idling_module_->getDebugMarkers(), &markers); + if (registered_modules_.empty() && idling_module_ptr_ != nullptr) { + appendMarkerArray(idling_module_ptr_->getInfoMarkers(), &info_markers); + appendMarkerArray(idling_module_ptr_->getDebugMarkers(), &debug_markers); } - pub_debug_marker_->publish(markers); + pub_info_marker_->publish(info_markers); + pub_debug_marker_->publish(debug_markers); } bool exist(const SceneModulePtr & module_ptr) const @@ -146,12 +203,18 @@ class SceneModuleManagerInterface bool isSimultaneousExecutableAsApprovedModule() const { - return enable_simultaneous_execution_as_approved_module_; + return std::all_of( + registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { + return module->isSimultaneousExecutableAsApprovedModule(); + }); } bool isSimultaneousExecutableAsCandidateModule() const { - return enable_simultaneous_execution_as_candidate_module_; + return std::all_of( + registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { + return module->isSimultaneousExecutableAsCandidateModule(); + }); } void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } @@ -164,9 +227,11 @@ class SceneModuleManagerInterface }); registered_modules_.clear(); - idling_module_->onExit(); - idling_module_->publishRTCStatus(); - idling_module_.reset(); + if (idling_module_ptr_ != nullptr) { + idling_module_ptr_->onExit(); + idling_module_ptr_->publishRTCStatus(); + idling_module_ptr_.reset(); + } pub_debug_marker_->publish(MarkerArray{}); } @@ -188,15 +253,19 @@ class SceneModuleManagerInterface rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_info_marker_; + rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + std::string name_; std::shared_ptr planner_data_; std::vector registered_modules_; - SceneModulePtr idling_module_; + SceneModulePtr idling_module_ptr_; std::unordered_map> rtc_interface_ptr_map_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp index 307f92e8ebbb6..03473709446c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp @@ -28,15 +28,13 @@ namespace behavior_path_planner // Forward Declaration class AvoidanceModule; class AvoidanceByLCModule; -#ifdef USE_OLD_ARCHITECTURE class LaneChangeModule; class ExternalRequestLaneChangeModule; -#else class LaneChangeInterface; -#endif +class LaneChangeBTInterface; class LaneFollowingModule; class PullOutModule; -class PullOverModule; +class GoalPlannerModule; class SideShiftModule; using tier4_planning_msgs::msg::AvoidanceDebugMsg; @@ -47,12 +45,10 @@ using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; class SceneModuleVisitor { public: -#ifdef USE_OLD_ARCHITECTURE void visitLaneChangeModule(const LaneChangeModule * module) const; void visitExternalRequestLaneChangeModule(const ExternalRequestLaneChangeModule * module) const; -#else void visitLaneChangeInterface(const LaneChangeInterface * interface) const; -#endif + void visitLaneChangeBTInterface(const LaneChangeBTInterface * module) const; void visitAvoidanceModule(const AvoidanceModule * module) const; void visitAvoidanceByLCModule(const AvoidanceByLCModule * module) const; @@ -61,10 +57,8 @@ class SceneModuleVisitor protected: mutable std::shared_ptr lane_change_visitor_; - -#ifdef USE_OLD_ARCHITECTURE mutable std::shared_ptr ext_request_lane_change_visitor_; -#endif + mutable std::shared_ptr external_request_lane_change_bt_visitor_; mutable std::shared_ptr avoidance_visitor_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 4683bd8477e6c..250ece8190e37 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -57,6 +57,7 @@ class SideShiftModule : public SceneModuleInterface const double & min_request_time_sec, bool override_requests = false) const noexcept; ModuleStatus updateState() override; void updateData() override; + ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -85,7 +86,7 @@ class SideShiftModule : public SceneModuleInterface #endif // non-const methods - void adjustDrivableArea(ShiftedPath * path) const; + BehaviorModuleOutput adjustDrivableArea(const ShiftedPath & path) const; ShiftLine calcShiftLine() const; @@ -126,13 +127,15 @@ class SideShiftModule : public SceneModuleInterface ShiftedPath prev_output_; ShiftLine prev_shift_line_; - // NOTE: this function is ported from avoidance. - Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const; - PathWithLaneId calcCenterLinePath( - const std::shared_ptr & planner_data, const Pose & pose) const; mutable rclcpp::Time last_requested_shift_change_time_{clock_->now()}; + + rclcpp::Time latest_lateral_offset_stamp_; + + // debug + mutable SideShiftDebugData debug_data_; + void setDebugMarkersVisualization() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index f702fe1b4c8e3..5691c03d31f6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ #define BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ -#include +#include #include #include @@ -45,18 +45,42 @@ const std::map signal_map = { {"straight", TurnIndicatorsCommand::DISABLE}, {"none", TurnIndicatorsCommand::DISABLE}}; +struct TurnSignalInfo +{ + TurnSignalInfo() + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + } + + // desired turn signal + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + + geometry_msgs::msg::Pose desired_start_point; + geometry_msgs::msg::Pose desired_end_point; + geometry_msgs::msg::Pose required_start_point; + geometry_msgs::msg::Pose required_end_point; +}; + class TurnSignalDecider { public: TurnIndicatorsCommand getTurnSignal( - const std::shared_ptr & planner_data, const PathWithLaneId & path, - const TurnSignalInfo & turn_signal_info); + const std::shared_ptr & route_handler, const PathWithLaneId & path, + const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, + const BehaviorPathPlannerParameters & parameters); TurnIndicatorsCommand resolve_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo use_prior_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold); + void setParameters( const double base_link2front, const double intersection_search_distance, const double intersection_search_time, const double intersection_angle_threshold_deg) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index aa2eef71384fc..a3c1a8b4e46f7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -94,6 +94,9 @@ struct AvoidanceParameters // disable path update bool disable_path_update{false}; + // use hatched road markings for avoidance + bool use_hatched_road_markings{false}; + // constrains bool use_constraints_for_decel{false}; @@ -116,8 +119,14 @@ struct AvoidanceParameters // vehicles with speed greater than this will not be avoided double threshold_speed_object_is_stopped; - // execute only when there is no intersection or crosswalk behind of the stopped vehicle. - double object_check_force_avoidance_clearance; + // execute only when there is no intersection behind of the stopped vehicle. + double object_ignore_distance_traffic_light; + + // execute only when there is no crosswalk near the stopped vehicle. + double object_ignore_distance_crosswalk_forward; + + // execute only when there is no crosswalk near the stopped vehicle. + double object_ignore_distance_crosswalk_backward; // distance to avoid object detection double object_check_forward_distance; @@ -144,9 +153,6 @@ struct AvoidanceParameters // we want to keep this lateral margin when avoiding double lateral_collision_margin; - // if object overhang is less than this value, the ego stops behind the object. - double lateral_passable_safety_buffer{0.5}; - // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction double longitudinal_collision_margin_min_distance; @@ -236,7 +242,22 @@ struct AvoidanceParameters // avoidance points is greater than this threshold. // In multiple targets case: if there are multiple vehicles in a row to be avoided, no new // avoidance path will be generated unless their lateral margin difference exceeds this value. - double avoidance_execution_lateral_threshold; + double lateral_execution_threshold; + + // For shift line generation process. The continuous shift length is quantized by this value. + double quantize_filter_threshold; + + // For shift line generation process. Merge small shift lines. (First step) + double same_grad_filter_1_threshold; + + // For shift line generation process. Merge small shift lines. (Second step) + double same_grad_filter_2_threshold; + + // For shift line generation process. Merge small shift lines. (Third step) + double same_grad_filter_3_threshold; + + // For shift line generation process. Remove sharp(=jerky) shift line. + double sharp_shift_filter_threshold; // target velocity matrix std::vector target_velocity_matrix; @@ -247,11 +268,6 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; - // drivable area expansion - double drivable_area_right_bound_offset{}; - double drivable_area_left_bound_offset{}; - std::vector drivable_area_types_to_skip{}; - // clip left and right bounds for objects bool enable_bound_clipping{false}; @@ -313,7 +329,7 @@ struct ObjectData // avoidance target double to_road_shoulder_distance{0.0}; // to intersection - double to_stop_factor_distance{std::numeric_limits::max()}; + double to_stop_factor_distance{std::numeric_limits::infinity()}; // if lateral margin is NOT enough, the ego must avoid the object. bool avoid_required{false}; @@ -383,6 +399,9 @@ struct AvoidancePlanningData // reference path (before shifting) PathWithLaneId reference_path; + // reference path (pre-resampled reference path) + PathWithLaneId reference_path_rough; + // closest reference_path index for reference_pose size_t ego_closest_path_index; @@ -500,10 +519,6 @@ struct DebugData std::vector total_shift; std::vector output_shift; - boost::optional stop_pose{boost::none}; - boost::optional slow_pose{boost::none}; - boost::optional feasible_bound{boost::none}; - bool exist_adjacent_objects{false}; // future pose diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp similarity index 76% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index f61f4ee243b38..d5ab62ddc23e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" @@ -22,34 +22,10 @@ #include #include -namespace behavior_path_planner +namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; -struct PolygonPoint -{ - geometry_msgs::msg::Point point; - size_t bound_seg_idx; - double lon_dist_to_segment; - double lat_dist_to_bound; - - bool is_after(const PolygonPoint & other_point) const - { - if (bound_seg_idx == other_point.bound_seg_idx) { - return other_point.lon_dist_to_segment < lon_dist_to_segment; - } - return other_point.bound_seg_idx < bound_seg_idx; - } - - bool is_outside_bounds(const bool is_on_right) const - { - if (is_on_right) { - return lat_dist_to_bound < 0.0; - } - return 0.0 < lat_dist_to_bound; - }; -}; - bool isOnRight(const ObjectData & obj); bool isTargetObjectType( @@ -58,6 +34,8 @@ bool isTargetObjectType( double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); +bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); + bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length); size_t findPathIndexFromArclength( @@ -89,11 +67,9 @@ void setStartData( Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); -void generateDrivableArea( - PathWithLaneId & path, const std::vector & lanes, - const std::shared_ptr planner_data, - const std::shared_ptr & parameters, const ObjectDataArray & objects, - const double vehicle_length, const bool enable_bound_clipping, const bool disable_path_update); +std::vector generateObstaclePolygonsForDrivableArea( + const ObjectDataArray & objects, const std::shared_ptr & parameters, + const double vehicle_width); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); @@ -128,6 +104,6 @@ void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -} // namespace behavior_path_planner +} // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTIL_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp deleted file mode 100644 index e556c56eac8d7..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ - -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" - -#include - -namespace behavior_path_planner -{ - -struct AvoidanceByLCParameters -{ - // avoidance - std::shared_ptr avoidance; - - // lane change - std::shared_ptr lane_change; - - // execute if the target object number is larger than this param. - size_t execute_object_num{1}; - - // execute only when the target object longitudinal distance is larger than this param. - double execute_object_longitudinal_margin{0.0}; - - // execute only when lane change end point is before the object. - bool execute_only_when_lane_change_finish_before_object{false}; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index b8d8a235580e4..a4c11c68d13ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -28,6 +28,9 @@ namespace drivable_area_expansion struct DrivableAreaExpansionParameters { + static constexpr auto DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM = "drivable_area_right_bound_offset"; + static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; + static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; static constexpr auto EGO_EXTRA_OFFSET_FRONT = "dynamic_expansion.ego.extra_footprint_offset.front"; @@ -55,6 +58,9 @@ struct DrivableAreaExpansionParameters static constexpr auto EXTRA_COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.extra_distance"; + double drivable_area_right_bound_offset; + double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip; bool enabled = false; std::string expansion_method{}; double avoid_linestring_dist{}; @@ -83,6 +89,12 @@ struct DrivableAreaExpansionParameters void init(rclcpp::Node & node) { + drivable_area_right_bound_offset = + node.declare_parameter(DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM); + drivable_area_left_bound_offset = + node.declare_parameter(DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM); + drivable_area_types_to_skip = + node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index b5cc03897e2fd..18bf7ad54e70c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -49,17 +49,27 @@ struct ParallelParkingParameters { double th_arrived_distance; double th_stopped_velocity; + double maximum_deceleration; + + // forward parking double after_forward_parking_straight_distance; - double after_backward_parking_straight_distance; double forward_parking_velocity; + double forward_parking_lane_departure_margin; + double forward_parking_path_interval; + double forward_parking_max_steer_angle; + + // backward parking + double after_backward_parking_straight_distance; double backward_parking_velocity; - double departing_velocity; double backward_parking_lane_departure_margin; - double forward_parking_lane_departure_margin; - double departing_lane_departure_margin; - double arc_path_interval; - double maximum_deceleration; - double max_steer_angle; + double backward_parking_path_interval; + double backward_parking_max_steer_angle; + + // pull_out + double pull_out_velocity; + double pull_out_lane_departure_margin; + double pull_out_path_interval; + double pull_out_max_steer_angle; }; class GeometricParallelParking @@ -72,9 +82,14 @@ class GeometricParallelParking bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes); - void setData( - const std::shared_ptr & planner_data, - const ParallelParkingParameters & parameters); + void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } + void setPlannerData(const std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + void setTurningRadius( + const BehaviorPathPlannerParameters & common_params, const double max_steer_angle); + void incrementPathIndex(); std::vector getArcPaths() const { return arc_paths_; } @@ -103,10 +118,11 @@ class GeometricParallelParking std::vector planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_r, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - bool is_forward, const double end_pose_offset, const double lane_departure_margin); + bool is_forward, const double end_pose_offset, const double lane_departure_margin, + const double arc_path_interval); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, - const bool is_left_turn, const bool is_forward); + const double arc_path_interval, const bool is_left_turn, const bool is_forward); PathPointWithLaneId generateArcPathPoint( const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp new file mode 100644 index 0000000000000..568ecdc98c705 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ + +#include "behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ + +class DefaultFixedGoalPlanner : public FixedGoalPlannerBase +{ +public: + DefaultFixedGoalPlanner() = default; + BehaviorModuleOutput plan(const std::shared_ptr & planner_data) const override; + +protected: +#ifdef USE_OLD_ARCHITECTURE + boost::optional getReferencePath( + const std::shared_ptr & planner_data) const; +#endif + PathWithLaneId modifyPathForSmoothGoalConnection( + const PathWithLaneId & path, const std::shared_ptr & planner_data) const; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp new file mode 100644 index 0000000000000..3bc0e960fe468 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/parameters.hpp" + +#include +#include + +#include +#include + +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LinearRing2d; + +namespace behavior_path_planner +{ + +class FixedGoalPlannerBase +{ +public: + FixedGoalPlannerBase() = default; + virtual ~FixedGoalPlannerBase() = default; + + virtual BehaviorModuleOutput plan( + const std::shared_ptr & planner_data) const = 0; + + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + +protected: + BehaviorModuleOutput previous_module_output_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index d8f671163722e..1378be5461a64 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp" +#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" #include #include @@ -36,7 +36,7 @@ class FreespacePullOver : public PullOverPlannerBase { public: FreespacePullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } @@ -50,4 +50,4 @@ class FreespacePullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp index 8a47a2a696ebd..aee78930d2e1f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp" #include @@ -33,8 +33,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr occupancy_grid_map, const bool is_forward); @@ -64,9 +63,10 @@ class GeometricPullOver : public PullOverPlannerBase LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_; bool is_forward_{true}; + bool left_side_parking_{true}; GeometricParallelParking planner_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp similarity index 61% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 279c0b143861e..6da7c131a281f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -13,8 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ + +#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include #include @@ -30,19 +32,22 @@ using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStarParam; -struct PullOverParameters +enum class ParkingPolicy { + LEFT_SIDE = 0, + RIGHT_SIDE, +}; + +struct GoalPlannerParameters { + // general params double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; - double margin_from_boundary; - double decide_path_distance; - double maximum_deceleration; - double maximum_jerk; - // goal research - std::string search_priority; // "efficient_path" or "close_goal" - bool enable_goal_research; + + // goal search + std::string search_priority; // "efficient_path" or "close_goal" + ParkingPolicy parking_policy; // "left_side" or "right_side" double forward_goal_search_length; double backward_goal_search_length; double goal_search_interval; @@ -50,65 +55,51 @@ struct PullOverParameters double max_lateral_offset; double lateral_offset_interval; double ignore_distance_from_lane_start; + double margin_from_boundary; + // occupancy grid map bool use_occupancy_grid; bool use_occupancy_grid_for_longitudinal_margin; double occupancy_grid_collision_check_margin; int theta_size; int obstacle_threshold; + // object recognition bool use_object_recognition; double object_recognition_collision_check_margin; + + // pull over general params + double pull_over_velocity; + double pull_over_minimum_velocity; + double decide_path_distance; + double maximum_deceleration; + double maximum_jerk; + // shift path bool enable_shift_parking; - int pull_over_sampling_num; + int shift_sampling_num; double maximum_lateral_jerk; double minimum_lateral_jerk; double deceleration_interval; - double pull_over_velocity; - double pull_over_minimum_velocity; - double after_pull_over_straight_distance; + double after_shift_straight_distance; + // parallel parking bool enable_arc_forward_parking; bool enable_arc_backward_parking; - double after_forward_parking_straight_distance; - double after_backward_parking_straight_distance; - double forward_parking_velocity; - double backward_parking_velocity; - double forward_parking_lane_departure_margin; - double backward_parking_lane_departure_margin; - double arc_path_interval; - double pull_over_max_steer_angle; + ParallelParkingParameters parallel_parking_parameters; + // freespace parking bool enable_freespace_parking; - // hazard - double hazard_on_threshold_distance; - double hazard_on_threshold_velocity; - // check safety with dynamic objects. Not used now. - double pull_over_duration; - double pull_over_prepare_duration; - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; - double prediction_time_resolution; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; - // debug - bool print_debug_info; - - // freespace pull over - std::string algorithm; + std::string freespace_parking_algorithm; double freespace_parking_velocity; double vehicle_shape_margin; - PlannerCommonParam common_parameters; + PlannerCommonParam freespace_parking_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // debug + bool print_debug_info; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 36abe47a3ebe6..be6e1c0046449 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/goal_searcher_base.hpp" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" @@ -33,7 +33,7 @@ class GoalSearcher : public GoalSearcherBase { public: GoalSearcher( - const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); GoalCandidates search(const Pose & original_goal_pose) override; @@ -49,7 +49,8 @@ class GoalSearcher : public GoalSearcherBase LinearRing2d vehicle_footprint_{}; std::shared_ptr occupancy_grid_map_{}; + bool left_side_parking_{true}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index 8bdbca45f4bb5..bab13a287960a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/parameters.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include @@ -54,7 +54,7 @@ using GoalCandidates = std::vector; class GoalSearcherBase { public: - explicit GoalSearcherBase(const PullOverParameters & parameters) { parameters_ = parameters; } + explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } virtual ~GoalSearcherBase() = default; void setPlannerData(const std::shared_ptr & planner_data) @@ -67,10 +67,10 @@ class GoalSearcherBase virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: - PullOverParameters parameters_; + GoalPlannerParameters parameters_; std::shared_ptr planner_data_; MultiPolygon2d area_polygons_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index dbf50363676f1..4b800917d4aec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include #include @@ -87,7 +87,7 @@ struct PullOverPath class PullOverPlannerBase { public: - PullOverPlannerBase(rclcpp::Node & node, const PullOverParameters & parameters) + PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); vehicle_footprint_ = createVehicleFootprint(vehicle_info_); @@ -107,8 +107,8 @@ class PullOverPlannerBase std::shared_ptr planner_data_; vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - PullOverParameters parameters_; + GoalPlannerParameters parameters_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp similarity index 85% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp index d42eb7f656bf9..445161f631bb7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp" #include @@ -33,7 +33,7 @@ class ShiftPullOver : public PullOverPlannerBase { public: ShiftPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr & occupancy_grid_map); @@ -58,7 +58,8 @@ class ShiftPullOver : public PullOverPlannerBase std::shared_ptr occupancy_grid_map_{}; static constexpr double resample_interval_{1.0}; + bool left_side_parking_{true}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 21cfa8f8e0cfb..b1f548425d48f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ -#include "behavior_path_planner/utils/pull_over/goal_searcher_base.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -34,7 +34,7 @@ namespace behavior_path_planner { -namespace pull_over_utils +namespace goal_planner_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; @@ -46,7 +46,7 @@ using visualization_msgs::msg::MarkerArray; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler); +lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); @@ -61,7 +61,7 @@ MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); -} // namespace pull_over_utils +} // namespace goal_planner_utils } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 1481e0bd3789d..2b5e11238383c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -14,11 +14,14 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include #include +#include #include namespace behavior_path_planner @@ -26,16 +29,11 @@ namespace behavior_path_planner struct LaneChangeParameters { // trajectory generation - double prepare_duration{2.0}; - double lane_changing_lateral_jerk{0.5}; - double lane_changing_lateral_acc{0.315}; - double lane_changing_lateral_acc_at_low_velocity{0.15}; - double lateral_acc_switching_velocity{0.4}; + double backward_lane_length{200.0}; double lane_change_finish_judge_buffer{3.0}; - double minimum_lane_changing_velocity{5.6}; double prediction_time_resolution{0.5}; - double maximum_deceleration{1.0}; - int lane_change_sampling_num{10}; + int longitudinal_acc_sampling_num{10}; + int lateral_acc_sampling_num{10}; // collision check bool enable_prepare_segment_collision_check{true}; @@ -57,14 +55,10 @@ struct LaneChangeParameters bool enable_cancel_lane_change{true}; bool enable_abort_lane_change{false}; - double abort_delta_time{3.0}; + double abort_delta_time{1.0}; + double aborting_time{5.0}; double abort_max_lateral_jerk{10.0}; - // drivable area expansion - double drivable_area_right_bound_offset{0.0}; - double drivable_area_left_bound_offset{0.0}; - std::vector drivable_area_types_to_skip{}; - // debug marker bool publish_debug_marker{false}; }; @@ -91,7 +85,35 @@ struct LaneChangeTargetObjectIndices std::vector other_lane{}; }; -enum class LaneChangeModuleType { NORMAL = 0, EXTERNAL_REQUEST }; +enum class LaneChangeModuleType { + NORMAL = 0, + EXTERNAL_REQUEST, + AVOIDANCE_BY_LANE_CHANGE, +}; + +struct AvoidanceByLCParameters +{ + std::shared_ptr avoidance{}; + std::shared_ptr lane_change{}; + + // execute if the target object number is larger than this param. + size_t execute_object_num{1}; + + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; +}; } // namespace behavior_path_planner +namespace behavior_path_planner::data::lane_change +{ +struct PathSafetyStatus +{ + bool is_safe{true}; + bool is_object_coming_from_rear{false}; +}; +} // namespace behavior_path_planner::data::lane_change + #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index ca4cd087dbf95..eb81c92eb80a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -42,12 +42,35 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using data::lane_change::PathSafetyStatus; +using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; +double calcLaneChangeResampleInterval( + const double lane_changing_length, const double lane_changing_velocity); + +double calcMaximumAcceleration( + const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, + const BehaviorPathPlannerParameters & params); + +void setPrepareVelocity( + PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); + +std::vector getAccelerationValues( + const double min_acc, const double max_acc, const size_t sampling_num); + +std::vector replaceWithSortedIds( + const std::vector & original_lane_ids, + const std::vector> & sorted_lane_ids); + +std::vector> getSortedLaneIds( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const double rough_shift_length); + PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); bool isPathInLanelets( @@ -55,54 +78,37 @@ bool isPathInLanelets( const lanelet::ConstLanelets & target_lanelets); double calcLaneChangingLength( - const double lane_changing_velocity, const double shift_length, - const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param); + const double lane_changing_velocity, const double shift_length, const double lateral_acc, + const double lateral_jerk); std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const std::vector> & sorted_lane_ids, const double acceleration, - const LaneChangePhaseInfo lane_change_length, const LaneChangePhaseInfo lane_change_velocity, + const std::vector> & sorted_lane_ids, const double longitudinal_acceleration, + const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length, + const LaneChangePhaseInfo lane_change_velocity, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_param); -bool getLaneChangePaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data); - -bool getLaneChangePaths( - const PathWithLaneId & original_path, const RouteHandler & route_handler, - const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, const Direction direction, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data); - -bool isLaneChangePathSafe( +PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::LaneChangeParameters & lane_change_parameter, - const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, + const double front_decel, const double rear_decel, std::unordered_map & debug_data, const double acceleration = 0.0); -#ifdef USE_OLD_ARCHITECTURE -bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const RouteHandler & route_handler, const double minimum_lane_change_length); -#else +bool isObjectIndexIncluded( + const size_t & index, const std::vector & dynamic_objects_indices); + bool hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const RouteHandler & route_handler, const double minimum_lane_change_length, - const Direction direction); -#endif + const RouteHandler & route_handler, const double minimum_lane_changing_velocity, + const BehaviorPathPlannerParameters & common_parameters, + const Direction direction = Direction::NONE); ShiftLine getLaneChangingShiftLine( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, @@ -112,17 +118,8 @@ PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route); - -PathWithLaneId getPrepareSegment( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double prepare_velocity); - -PathWithLaneId getPrepareSegment( - const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets, - const Pose & current_pose, const double backward_path_length, const double prepare_length, - const double prepare_velocity); + const double resample_interval, const bool is_goal_in_route, + const double next_lane_change_buffer); PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, @@ -145,24 +142,15 @@ std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); -std::vector combineDrivableLanes( - const std::vector & original_drivable_lanes_vec, - const std::vector & new_drivable_lanes_vec); - -std::optional getAbortPaths( - const std::shared_ptr & planner_data, const LaneChangePath & selected_path, - const Pose & ego_lerp_pose_before_collision, const BehaviorPathPlannerParameters & common_param, - const LaneChangeParameters & lane_change_param); - double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & curent_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param); + const BehaviorPathPlannerParameters & common_param, const Direction direction); -lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( - const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lanes, +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); LaneChangeTargetObjectIndices filterObjectIndices( @@ -175,11 +163,14 @@ bool isTargetObjectType(const PredictedObject & object, const LaneChangeParamete double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); +double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); + std::string getStrDirection(const std::string & name, const Direction direction); -lanelet::ConstLanelets getLaneChangeLanes( - const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length, - const double prepare_duration, const Direction direction, const LaneChangeModuleType type); +CandidateOutput assignToCandidate( + const LaneChangePath & lane_change_path, const Point & ego_position); +boost::optional getLaneChangeTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType type, const Direction & direction); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp index d4eab4835dddb..118fb39606012 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp @@ -24,10 +24,6 @@ namespace behavior_path_planner struct LaneFollowingParameters { double lane_change_prepare_duration; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip{}; // finding closest lanelet double distance_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index b5b1e5af5c328..76119a18646ab 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -78,9 +78,14 @@ class PathShifter void setVelocity(const double velocity); /** - * @brief Set acceleration limit + * @brief Set lateral acceleration limit */ - void setLateralAccelerationLimit(const double acc); + void setLateralAccelerationLimit(const double lateral_acc); + + /** + * @brief Set longitudinal acceleration + */ + void setLongitudinalAcceleration(const double longitudinal_acc); /** * @brief Add shift point. You don't have to care about the start/end_idx. @@ -165,7 +170,9 @@ class PathShifter double velocity_{0.0}; // lateral acceleration limit considered in the path planning - double acc_limit_{-1.0}; + double lateral_acc_limit_{-1.0}; + + double longitudinal_acc_{0.0}; // Logger mutable rclcpp::Logger logger_{ @@ -180,6 +187,10 @@ class PathShifter std::pair, std::vector> getBaseLengthsWithoutAccelLimit( const double arclength, const double shift_length, const bool offset_back) const; + std::pair, std::vector> getBaseLengthsWithoutAccelLimit( + const double arclength, const double shift_length, const double velocity, + const double longitudinal_acc, const double total_time, const bool offset_back) const; + /** * @brief Calculate path index for shift_lines and set is_index_aligned_ to true. */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index 8ae184ae2a429..bdca6a2fdf085 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include #include #include @@ -28,6 +29,7 @@ #include #include +#include #include #include @@ -92,6 +94,13 @@ std::vector splineTwoPoints( std::vector interpolatePose( const Pose & start_pose, const Pose & end_pose, const double resample_interval); +geometry_msgs::msg::Pose getUnshiftedEgoPose( + const geometry_msgs::msg::Pose & ego_pose, const ShiftedPath & prev_path); + +PathWithLaneId calcCenterLinePath( + const std::shared_ptr & planner_data, const Pose & ref_pose, + const double longest_dist_to_shift_line, + const std::optional & prev_module_path = std::nullopt); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp index 788b37448c4a8..0ab6bd465fb26 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp @@ -26,9 +26,7 @@ namespace behavior_path_planner class GeometricPullOut : public PullOutPlannerBase { public: - explicit GeometricPullOut( - rclcpp::Node & node, const PullOutParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters); + explicit GeometricPullOut(rclcpp::Node & node, const PullOutParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; boost::optional plan(Pose start_pose, Pose goal_pose) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp index 49a72d4b79524..0fc7d367718ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp @@ -16,6 +16,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ +#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" + #include #include @@ -39,22 +41,15 @@ struct PullOutParameters // geometric pull out bool enable_geometric_pull_out; bool divide_pull_out_path; - double geometric_pull_out_velocity; - double arc_path_interval; - double lane_departure_margin; - double backward_velocity; - double pull_out_max_steer_angle; + ParallelParkingParameters parallel_parking_parameters; // search start pose backward std::string search_priority; // "efficient_path" or "short_back_distance" bool enable_back; + double backward_velocity; double max_back_distance; double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp index dc6bcfa0ed52a..ed7faa953cf0a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp @@ -42,6 +42,7 @@ namespace behavior_path_planner::utils::safety_check using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; @@ -55,50 +56,24 @@ struct ProjectedDistancePoint double distance{0.0}; }; -/** - * @brief Project nearest point on a line segment. - * @param [in] reference_point point to project - * @param [in] line segment - * @return nearest point on the line segment - */ -template > -ProjectedDistancePoint pointToSegment( - const Point2d & reference_point, const Point2d & polygon_segment_start, - const Point2d & polygon_segment_end); +bool isTargetObjectFront( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); -/** - * @brief Find nearest points between two polygon. - */ -void getProjectedDistancePointFromPolygons( - const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, - Pose & point_on_object); +Polygon2d createExtendedPolygon( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const double lon_length, const double lat_margin); +Polygon2d createExtendedPolygon( + const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin); -/** - * @brief get relative pose with reference to the target object. - * @param [in] absolute pose desired_pose reference pose - * @param [in] absolute pose target_pose target pose to check - * @return relative pose of the target - */ -Pose projectCurrentPoseToTarget(const Pose & reference_pose, const Pose & target_pose); +double calcRssDistance( + const double front_object_velocity, const double rear_object_velocity, + const double front_object_deceleration, const double rear_object_deceleration, + const BehaviorPathPlannerParameters & params); -/** - * @brief find which vehicle is front and rear and check for lateral, - * longitudinal physical and longitudinal expected stopping distance between two points - * @param [in] expected_ego_pose ego vehicle's pose - * @param [in] ego_current_twist ego vehicle's twist - * @param [in] expected_object_pose object vehicle's pose - * @param [in] object_current_twist object vehicle's twist - * @param [in] param common behavior path planner parameters - * @param [in] front_decel expected deceleration of front vehicle - * @param [in] rear_decel expected deceleration of rear vehicle - * @param [in] debug debug data - * @return true if distance is safe. - */ -bool hasEnoughDistance( - const Pose & expected_ego_pose, const Twist & ego_current_twist, - const Pose & expected_object_pose, const Twist & object_current_twist, - const BehaviorPathPlannerParameters & param, const double front_decel, const double rear_decel, - CollisionCheckDebug & debug); +double calcMinimumLongitudinalLength( + const double front_object_velocity, const double rear_object_velocity, + const BehaviorPathPlannerParameters & params); /** * @brief Iterate the points in the ego and target's predicted path and @@ -106,12 +81,13 @@ bool hasEnoughDistance( * @return true if distance is safe. */ bool isSafeInLaneletCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug); + const double rear_decel, CollisionCheckDebug & debug); /** * @brief Iterate the points in the ego and target's predicted path and @@ -119,12 +95,13 @@ bool isSafeInLaneletCollisionCheck( * @return true if distance is safe. */ bool isSafeInFreeSpaceCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug); + const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug); } // namespace behavior_path_planner::utils::safety_check diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp index ee139b0774803..89696190059cc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp @@ -15,8 +15,11 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ +#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" + #include +#include #include #include @@ -37,10 +40,14 @@ struct SideShiftParameters double drivable_area_width; double drivable_area_height; double shift_request_time_limit; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; + bool publish_debug_marker; +}; + +struct SideShiftDebugData +{ + std::shared_ptr path_shifter{}; + ShiftLineArray shift_lines{}; + double current_request{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 655fbbd33bef6..87bced439ce46 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -23,7 +23,6 @@ #include "motion_utils/motion_utils.hpp" #include "perception_utils/predicted_path_utils.hpp" -#include #include #include @@ -77,6 +76,30 @@ using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; +struct PolygonPoint +{ + geometry_msgs::msg::Point point; + size_t bound_seg_idx; + double lon_dist_to_segment; + double lat_dist_to_bound; + + bool is_after(const PolygonPoint & other_point) const + { + if (bound_seg_idx == other_point.bound_seg_idx) { + return other_point.lon_dist_to_segment < lon_dist_to_segment; + } + return other_point.bound_seg_idx < bound_seg_idx; + } + + bool is_outside_bounds(const bool is_on_right) const + { + if (is_on_right) { + return lat_dist_to_bound < 0.0; + } + return 0.0 < lat_dist_to_bound; + }; +}; + struct FrenetPoint { double length{0.0}; // longitudinal @@ -206,18 +229,23 @@ boost::optional getLeftLanelet( std::vector generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); +std::vector calcBound( + const std::shared_ptr route_handler, + const std::vector & drivable_lanes, const bool enable_expanding_polygon, + const bool is_left); boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); void generateDrivableArea( - PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + PathWithLaneId & path, const std::vector & lanes, + const bool enable_expanding_polygon, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( - PathWithLaneId & path, const double vehicle_length, const double vehicle_width, - const double margin, const bool is_driving_forward = true); + PathWithLaneId & path, const double vehicle_length, const double offset, + const bool is_driving_forward = true); lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); @@ -269,6 +297,18 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); +BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & planner_data); + +bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); + +bool isInLaneletWithYawThreshold( + const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, + const double radius = 0.0); + +bool isEgoOutOfRoute( + const Pose & self_pose, const std::optional & modified_goal, + const std::shared_ptr & route_handler); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. @@ -277,13 +317,11 @@ std::shared_ptr generateCenterLinePath( PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); -double getSignedDistanceFromShoulderLeftBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose); -std::optional getSignedDistanceFromShoulderLeftBoundary( +double getSignedDistanceFromBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); +std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose); -double getSignedDistanceFromRightBoundary( - const lanelet::ConstLanelets & lanelets, const Pose & pose); + const Pose & vehicle_pose, const bool left_side); // misc @@ -306,13 +344,8 @@ PathWithLaneId setDecelerationVelocity( const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration, const double lane_change_buffer); -PathWithLaneId setDecelerationVelocity( - const PathWithLaneId & input, const double target_velocity, const Pose target_pose, - const double buffer, const double deceleration_interval); - BehaviorModuleOutput getReferencePath( const lanelet::ConstLanelet & current_lane, - const std::shared_ptr & parameters, const std::shared_ptr & planner_data); // object label @@ -343,17 +376,30 @@ boost::optional> getEgoExpectedPoseAndConvertToPolygo bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); -double calcTotalLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const bool include_buffer = true); - -double calcLaneChangeBuffer( - const BehaviorPathPlannerParameters & common_param, const int num_lane_change, +double calcMinimumLaneChangeLength( + const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, const double length_to_intersection = 0.0); lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); std::string convertToSnakeCase(const std::string & input_str); + +std::vector combineDrivableLanes( + const std::vector & original_drivable_lanes_vec, + const std::vector & new_drivable_lanes_vec); + +DrivableAreaInfo combineDrivableAreaInfo( + const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); + +void extractObstaclesFromDrivableArea( + PathWithLaneId & path, const std::vector & obstacles); + +void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left); + +std::optional getPolygonByPoint( + const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, + const std::string & polygon_name); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 376c468e2d605..b5585151869df 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -7,7 +7,7 @@ Zulfaqar Azmi - + Kosuke Takeuchi Kosuke Takeuchi @@ -39,8 +39,7 @@ Ryohsuke Mitsudome ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_perception_msgs @@ -54,7 +53,6 @@ lane_departure_checker lanelet2_extension libboost-dev - libopencv-dev magic_enum motion_utils perception_utils 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 b96566e9ed513..407f2d853f88a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" #include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" @@ -120,10 +121,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // set parameters { avoidance_param_ptr_ = std::make_shared(getAvoidanceParam()); + dynamic_avoidance_param_ptr_ = + std::make_shared(getDynamicAvoidanceParam()); lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); - lane_following_param_ptr_ = std::make_shared(getLaneFollowingParam()); pull_out_param_ptr_ = std::make_shared(getPullOutParam()); - pull_over_param_ptr_ = std::make_shared(getPullOverParam()); + goal_planner_param_ptr_ = std::make_shared(getGoalPlannerParam()); side_shift_param_ptr_ = std::make_shared(getSideShiftParam()); avoidance_by_lc_param_ptr_ = std::make_shared( getAvoidanceByLCParam(avoidance_param_ptr_, lane_change_param_ptr_)); @@ -152,12 +154,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "Avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); bt_manager_->registerSceneModule(avoidance_module); - auto lane_following_module = - std::make_shared("LaneFollowing", *this, lane_following_param_ptr_); + auto lane_following_module = std::make_shared("LaneFollowing", *this); bt_manager_->registerSceneModule(lane_following_module); auto ext_request_lane_change_right_module = - std::make_shared( + std::make_shared( "ExternalRequestLaneChangeRight", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "ExternalRequestLaneChangeRight", @@ -165,7 +166,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(ext_request_lane_change_right_module); auto ext_request_lane_change_left_module = - std::make_shared( + std::make_shared( "ExternalRequestLaneChangeLeft", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "ExternalRequestLaneChangeLeft", @@ -173,16 +174,16 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(ext_request_lane_change_left_module); auto lane_change_module = - std::make_shared("LaneChange", *this, lane_change_param_ptr_); + std::make_shared("LaneChange", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); bt_manager_->registerSceneModule(lane_change_module); - auto pull_over_module = - std::make_shared("PullOver", *this, pull_over_param_ptr_); + auto goal_planner = + std::make_shared("GoalPlanner", *this, goal_planner_param_ptr_); path_candidate_publishers_.emplace( - "PullOver", create_publisher(path_candidate_name_space + "pull_over", 1)); - bt_manager_->registerSceneModule(pull_over_module); + "GoalPlanner", create_publisher(path_candidate_name_space + "goal_planner", 1)); + bt_manager_->registerSceneModule(goal_planner); auto pull_out_module = std::make_shared("PullOut", *this, pull_out_param_ptr_); path_candidate_publishers_.emplace( @@ -201,8 +202,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ const auto & p = planner_data_->parameters; - planner_manager_ = - std::make_shared(*this, lane_following_param_ptr_, p.verbose); + planner_manager_ = std::make_shared(*this, p.verbose); const auto register_and_create_publisher = [&](const auto & manager) { const auto & module_name = manager->getModuleName(); @@ -223,14 +223,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "pull_out", create_publisher(path_reference_name_space + "pull_out", 1)); } - if (p.config_pull_over.enable_module) { - auto manager = std::make_shared( - this, "pull_over", p.config_pull_over, pull_over_param_ptr_); + if (p.config_goal_planner.enable_module) { + auto manager = std::make_shared( + this, "goal_planner", p.config_goal_planner, goal_planner_param_ptr_); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( - "pull_over", create_publisher(path_candidate_name_space + "pull_over", 1)); + "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); path_reference_publishers_.emplace( - "pull_over", create_publisher(path_reference_name_space + "pull_over", 1)); + "goal_planner", create_publisher(path_reference_name_space + "goal_planner", 1)); } if (p.config_side_shift.enable_module) { @@ -286,8 +286,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } if (p.config_avoidance_by_lc.enable_module) { - auto manager = std::make_shared( - this, "avoidance_by_lane_change", p.config_avoidance_by_lc, avoidance_by_lc_param_ptr_); + auto manager = std::make_shared( + this, "avoidance_by_lane_change", p.config_avoidance_by_lc, lane_change_param_ptr_, + avoidance_param_ptr_, avoidance_by_lc_param_ptr_); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "avoidance_by_lane_change", @@ -296,6 +297,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "avoidance_by_lane_change", create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); } + + if (p.config_dynamic_avoidance.enable_module) { + auto manager = std::make_shared( + this, "dynamic_avoidance", p.config_dynamic_avoidance, dynamic_avoidance_param_ptr_); + planner_manager_->registerSceneModuleManager(manager); + } } #endif @@ -306,7 +313,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const double turn_signal_intersection_angle_threshold_deg = planner_data_->parameters.turn_signal_intersection_angle_threshold_deg; const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - turn_signal_decider_.setParameters( + planner_data_->turn_signal_decider.setParameters( planner_data_->parameters.base_link2front, turn_signal_intersection_search_distance, turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } @@ -341,7 +348,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() }; p.config_pull_out = get_scene_module_manager_param("pull_out."); - p.config_pull_over = get_scene_module_manager_param("pull_over."); + p.config_goal_planner = get_scene_module_manager_param("goal_planner."); p.config_side_shift = get_scene_module_manager_param("side_shift."); p.config_lane_change_left = get_scene_module_manager_param("lane_change_left."); p.config_lane_change_right = get_scene_module_manager_param("lane_change_right."); @@ -351,6 +358,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() get_scene_module_manager_param("external_request_lane_change_left."); p.config_avoidance = get_scene_module_manager_param("avoidance."); p.config_avoidance_by_lc = get_scene_module_manager_param("avoidance_by_lc."); + p.config_dynamic_avoidance = get_scene_module_manager_param("dynamic_avoidance."); // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); @@ -379,15 +387,48 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // ROS parameters p.backward_path_length = declare_parameter("backward_path_length") + backward_offset; p.forward_path_length = declare_parameter("forward_path_length"); + + // acceleration parameters + p.min_acc = declare_parameter("normal.min_acc"); + p.max_acc = declare_parameter("normal.max_acc"); + + // lane change parameters p.backward_length_buffer_for_end_of_lane = declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); + p.lane_changing_lateral_jerk = + declare_parameter("lane_change.lane_changing_lateral_jerk"); + p.lateral_acc_switching_velocity = + declare_parameter("lane_change.lateral_acc_switching_velocity"); + p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); + p.minimum_lane_changing_velocity = + declare_parameter("lane_change.minimum_lane_changing_velocity"); + p.minimum_lane_changing_velocity = + std::min(p.minimum_lane_changing_velocity, p.max_acc * p.lane_change_prepare_duration); + p.minimum_prepare_length = + 0.5 * p.max_acc * p.lane_change_prepare_duration * p.lane_change_prepare_duration; + + // lateral acceleration map for lane change + const auto lateral_acc_velocity = + declare_parameter>("lane_change.lateral_acceleration.velocity"); + const auto min_lateral_acc = + declare_parameter>("lane_change.lateral_acceleration.min_values"); + const auto max_lateral_acc = + declare_parameter>("lane_change.lateral_acceleration.max_values"); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR(get_logger(), "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.lane_change_lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = declare_parameter("backward_length_buffer_for_end_of_pull_out"); - p.minimum_lane_changing_length = - declare_parameter("lane_change.minimum_lane_changing_length"); - p.minimum_prepare_length = declare_parameter("lane_change.minimum_prepare_length"); p.minimum_pull_over_length = declare_parameter("minimum_pull_over_length"); p.refine_goal_search_radius_range = declare_parameter("refine_goal_search_radius_range"); @@ -444,13 +485,8 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() p.shifting_lateral_jerk = declare_parameter(ns + "shifting_lateral_jerk"); p.min_shifting_distance = declare_parameter(ns + "min_shifting_distance"); p.min_shifting_speed = declare_parameter(ns + "min_shifting_speed"); - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); p.shift_request_time_limit = declare_parameter(ns + "shift_request_time_limit"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); + p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); return p; } @@ -472,6 +508,9 @@ AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam( declare_parameter(ns + "execute_only_when_lane_change_finish_before_object"); } + if (p.execute_object_num < 1) { + RCLCPP_WARN_STREAM(get_logger(), "execute_object_num cannot be lesser than 1."); + } return p; } @@ -490,12 +529,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "detection_area_right_expand_dist"); p.detection_area_left_expand_dist = declare_parameter(ns + "detection_area_left_expand_dist"); - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); p.enable_bound_clipping = declare_parameter(ns + "enable_bound_clipping"); p.enable_avoidance_over_same_direction = declare_parameter(ns + "enable_avoidance_over_same_direction"); @@ -508,6 +541,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); p.disable_path_update = declare_parameter(ns + "disable_path_update"); + p.use_hatched_road_markings = declare_parameter(ns + "use_hatched_road_markings"); p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); p.print_debug_info = declare_parameter(ns + "print_debug_info"); } @@ -545,8 +579,12 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "threshold_time_object_is_moving"); p.threshold_time_force_avoidance_for_stopped_vehicle = declare_parameter(ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_check_force_avoidance_clearance = - declare_parameter(ns + "object_check_force_avoidance_clearance"); + p.object_ignore_distance_traffic_light = + declare_parameter(ns + "object_ignore_distance_traffic_light"); + p.object_ignore_distance_crosswalk_forward = + declare_parameter(ns + "object_ignore_distance_crosswalk_forward"); + p.object_ignore_distance_crosswalk_backward = + declare_parameter(ns + "object_ignore_distance_crosswalk_backward"); p.object_check_forward_distance = declare_parameter(ns + "object_check_forward_distance"); p.object_check_backward_distance = @@ -576,11 +614,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { std::string ns = "avoidance.avoidance.lateral."; p.lateral_collision_margin = declare_parameter(ns + "lateral_collision_margin"); - p.lateral_passable_safety_buffer = - declare_parameter(ns + "lateral_passable_safety_buffer"); p.road_shoulder_safety_margin = declare_parameter(ns + "road_shoulder_safety_margin"); - p.avoidance_execution_lateral_threshold = - declare_parameter(ns + "avoidance_execution_lateral_threshold"); + p.lateral_execution_threshold = declare_parameter(ns + "lateral_execution_threshold"); p.max_right_shift_length = declare_parameter(ns + "max_right_shift_length"); p.max_left_shift_length = declare_parameter(ns + "max_left_shift_length"); } @@ -640,26 +675,60 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.target_velocity_matrix = declare_parameter>(ns + "matrix"); } + // shift line pipeline + { + std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = declare_parameter(ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + declare_parameter(ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + declare_parameter(ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + declare_parameter(ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + declare_parameter(ns + "trim.sharp_shift_filter_threshold"); + } + return p; } -LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() +DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() { - LaneFollowingParameters p{}; - p.drivable_area_right_bound_offset = - declare_parameter("lane_following.drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter("lane_following.drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>("lane_following.drivable_area_types_to_skip"); - p.lane_change_prepare_duration = - declare_parameter("lane_following.lane_change_prepare_duration"); - - // finding closest lanelet - { - p.distance_threshold = - declare_parameter("lane_following.closest_lanelet.distance_threshold"); - p.yaw_threshold = declare_parameter("lane_following.closest_lanelet.yaw_threshold"); + DynamicAvoidanceParameters p{}; + + { // target object + std::string ns = "dynamic_avoidance.target_object."; + p.avoid_car = declare_parameter(ns + "car"); + p.avoid_truck = declare_parameter(ns + "truck"); + p.avoid_bus = declare_parameter(ns + "bus"); + p.avoid_trailer = declare_parameter(ns + "trailer"); + p.avoid_unknown = declare_parameter(ns + "unknown"); + p.avoid_bicycle = declare_parameter(ns + "bicycle"); + p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); + p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); + p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel"); + } + + { // drivable_area_generation + std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.lat_offset_from_obstacle = declare_parameter(ns + "lat_offset_from_obstacle"); + p.max_lat_offset_to_avoid = declare_parameter(ns + "max_lat_offset_to_avoid"); + + p.max_time_to_collision_overtaking_object = + declare_parameter(ns + "overtaking_object.max_time_to_collision"); + p.start_duration_to_avoid_overtaking_object = + declare_parameter(ns + "overtaking_object.start_duration_to_avoid"); + p.end_duration_to_avoid_overtaking_object = + declare_parameter(ns + "overtaking_object.end_duration_to_avoid"); + p.duration_to_hold_avoidance_overtaking_object = + declare_parameter(ns + "overtaking_object.duration_to_hold_avoidance"); + + p.max_time_to_collision_oncoming_object = + declare_parameter(ns + "oncoming_object.max_time_to_collision"); + p.start_duration_to_avoid_oncoming_object = + declare_parameter(ns + "oncoming_object.start_duration_to_avoid"); + p.end_duration_to_avoid_oncoming_object = + declare_parameter(ns + "oncoming_object.end_duration_to_avoid"); } return p; @@ -671,20 +740,14 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation - p.prepare_duration = declare_parameter(parameter("prepare_duration")); - p.lane_changing_lateral_jerk = declare_parameter(parameter("lane_changing_lateral_jerk")); - p.lane_changing_lateral_acc = declare_parameter(parameter("lane_changing_lateral_acc")); - p.lane_changing_lateral_acc_at_low_velocity = - declare_parameter(parameter("lane_changing_lateral_acc_at_low_velocity")); - p.lateral_acc_switching_velocity = - declare_parameter(parameter("lateral_acc_switching_velocity")); + p.backward_lane_length = declare_parameter(parameter("backward_lane_length")); p.lane_change_finish_judge_buffer = declare_parameter(parameter("lane_change_finish_judge_buffer")); - p.minimum_lane_changing_velocity = - declare_parameter(parameter("minimum_lane_changing_velocity")); p.prediction_time_resolution = declare_parameter(parameter("prediction_time_resolution")); - p.maximum_deceleration = declare_parameter(parameter("maximum_deceleration")); - p.lane_change_sampling_num = declare_parameter(parameter("lane_change_sampling_num")); + p.longitudinal_acc_sampling_num = + declare_parameter(parameter("longitudinal_acceleration_sampling_num")); + p.lateral_acc_sampling_num = + declare_parameter(parameter("lateral_acceleration_sampling_num")); // collision check p.enable_prepare_segment_collision_check = @@ -713,32 +776,20 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.enable_abort_lane_change = declare_parameter(parameter("enable_abort_lane_change")); p.abort_delta_time = declare_parameter(parameter("abort_delta_time")); + p.aborting_time = declare_parameter(parameter("aborting_time")); p.abort_max_lateral_jerk = declare_parameter(parameter("abort_max_lateral_jerk")); - // drivable area expansion - p.drivable_area_right_bound_offset = - declare_parameter(parameter("drivable_area_right_bound_offset")); - p.drivable_area_left_bound_offset = - declare_parameter(parameter("drivable_area_left_bound_offset")); - p.drivable_area_types_to_skip = - declare_parameter>(parameter("drivable_area_types_to_skip")); - // debug marker p.publish_debug_marker = declare_parameter(parameter("publish_debug_marker")); // validation of parameters - if (p.lane_change_sampling_num < 1) { + if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { RCLCPP_FATAL_STREAM( - get_logger(), "lane_change_sampling_num must be positive integer. Given parameter: " - << p.lane_change_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); + get_logger(), + "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.longitudinal_acc_sampling_num + << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } @@ -761,22 +812,22 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() return p; } -PullOverParameters BehaviorPathPlannerNode::getPullOverParam() +GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() { - PullOverParameters p; + GoalPlannerParameters p; + // general params { - std::string ns = "pull_over."; + std::string ns = "goal_planner."; p.minimum_request_length = declare_parameter(ns + "minimum_request_length"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); - p.margin_from_boundary = declare_parameter(ns + "margin_from_boundary"); - p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); - p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); - p.maximum_jerk = declare_parameter(ns + "maximum_jerk"); - // goal research - p.enable_goal_research = declare_parameter(ns + "enable_goal_research"); + } + + // goal search + { + std::string ns = "goal_planner.goal_search."; p.search_priority = declare_parameter(ns + "search_priority"); p.forward_goal_search_length = declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = declare_parameter(ns + "backward_goal_search_length"); @@ -786,7 +837,24 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.lateral_offset_interval = declare_parameter(ns + "lateral_offset_interval"); p.ignore_distance_from_lane_start = declare_parameter(ns + "ignore_distance_from_lane_start"); - // occupancy grid map + p.margin_from_boundary = declare_parameter(ns + "margin_from_boundary"); + + const std::string parking_policy_name = declare_parameter(ns + "parking_policy"); + if (parking_policy_name == "left_side") { + p.parking_policy = ParkingPolicy::LEFT_SIDE; + } else if (parking_policy_name == "right_side") { + p.parking_policy = ParkingPolicy::RIGHT_SIDE; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); + exit(EXIT_FAILURE); + } + } + + // occupancy grid map + { + std::string ns = "goal_planner.occupancy_grid."; p.use_occupancy_grid = declare_parameter(ns + "use_occupancy_grid"); p.use_occupancy_grid_for_longitudinal_margin = declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); @@ -794,111 +862,118 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = declare_parameter(ns + "theta_size"); p.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); - // object recognition + } + + // object recognition + { + std::string ns = "goal_planner.object_recognition."; p.use_object_recognition = declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = declare_parameter(ns + "object_recognition_collision_check_margin"); - // shift path + } + + // pull over general params + { + std::string ns = "goal_planner.pull_over."; + p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); + p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); + p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); + p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); + p.maximum_jerk = declare_parameter(ns + "maximum_jerk"); + } + + // shift parking + { + std::string ns = "goal_planner.pull_over.shift_parking."; p.enable_shift_parking = declare_parameter(ns + "enable_shift_parking"); - p.pull_over_sampling_num = declare_parameter(ns + "pull_over_sampling_num"); + p.shift_sampling_num = declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = declare_parameter(ns + "maximum_lateral_jerk"); p.minimum_lateral_jerk = declare_parameter(ns + "minimum_lateral_jerk"); p.deceleration_interval = declare_parameter(ns + "deceleration_interval"); - p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); - p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); - p.after_pull_over_straight_distance = - declare_parameter(ns + "after_pull_over_straight_distance"); - // parallel parking + p.after_shift_straight_distance = + declare_parameter(ns + "after_shift_straight_distance"); + } + + // forward parallel parking forward + { + std::string ns = "goal_planner.pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = declare_parameter(ns + "enable_arc_forward_parking"); - p.enable_arc_backward_parking = declare_parameter(ns + "enable_arc_backward_parking"); - p.after_forward_parking_straight_distance = + p.parallel_parking_parameters.after_forward_parking_straight_distance = declare_parameter(ns + "after_forward_parking_straight_distance"); - p.after_backward_parking_straight_distance = - declare_parameter(ns + "after_backward_parking_straight_distance"); - p.forward_parking_velocity = declare_parameter(ns + "forward_parking_velocity"); - p.backward_parking_velocity = declare_parameter(ns + "backward_parking_velocity"); - p.forward_parking_lane_departure_margin = + p.parallel_parking_parameters.forward_parking_velocity = + declare_parameter(ns + "forward_parking_velocity"); + p.parallel_parking_parameters.forward_parking_lane_departure_margin = declare_parameter(ns + "forward_parking_lane_departure_margin"); - p.backward_parking_lane_departure_margin = - declare_parameter(ns + "backward_parking_lane_departure_margin"); - p.arc_path_interval = declare_parameter(ns + "arc_path_interval"); - p.pull_over_max_steer_angle = - declare_parameter(ns + "pull_over_max_steer_angle"); // 20deg - // freespace parking - p.enable_freespace_parking = declare_parameter(ns + "enable_freespace_parking"); - // hazard - p.hazard_on_threshold_distance = declare_parameter(ns + "hazard_on_threshold_distance"); - p.hazard_on_threshold_velocity = declare_parameter(ns + "hazard_on_threshold_velocity"); - // safety with dynamic objects. Not used now. - p.pull_over_duration = declare_parameter(ns + "pull_over_duration"); - p.pull_over_prepare_duration = declare_parameter(ns + "pull_over_prepare_duration"); - p.min_stop_distance = declare_parameter(ns + "min_stop_distance"); - p.stop_time = declare_parameter(ns + "stop_time"); - p.hysteresis_buffer_distance = declare_parameter(ns + "hysteresis_buffer_distance"); - p.prediction_time_resolution = declare_parameter(ns + "prediction_time_resolution"); - p.enable_collision_check_at_prepare_phase = - declare_parameter(ns + "enable_collision_check_at_prepare_phase"); - p.use_predicted_path_outside_lanelet = - declare_parameter(ns + "use_predicted_path_outside_lanelet"); - p.use_all_predicted_path = declare_parameter(ns + "use_all_predicted_path"); - // drivable area - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); - // debug - p.print_debug_info = declare_parameter(ns + "print_debug_info"); + p.parallel_parking_parameters.forward_parking_path_interval = + declare_parameter(ns + "forward_parking_path_interval"); + p.parallel_parking_parameters.forward_parking_max_steer_angle = + declare_parameter(ns + "forward_parking_max_steer_angle"); // 20deg + } - // validation of parameters - if (p.pull_over_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - get_logger(), "pull_over_sampling_num must be positive integer. Given parameter: " - << p.pull_over_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } + // forward parallel parking backward + { + std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + p.enable_arc_backward_parking = declare_parameter(ns + "enable_arc_backward_parking"); + p.parallel_parking_parameters.after_backward_parking_straight_distance = + declare_parameter(ns + "after_backward_parking_straight_distance"); + p.parallel_parking_parameters.backward_parking_velocity = + declare_parameter(ns + "backward_parking_velocity"); + p.parallel_parking_parameters.backward_parking_lane_departure_margin = + declare_parameter(ns + "backward_parking_lane_departure_margin"); + p.parallel_parking_parameters.backward_parking_path_interval = + declare_parameter(ns + "backward_parking_path_interval"); + p.parallel_parking_parameters.backward_parking_max_steer_angle = + declare_parameter(ns + "backward_parking_max_steer_angle"); // 20deg } + // freespace parking general params { - std::string ns = "pull_over.freespace_parking."; - // search configs - p.algorithm = declare_parameter(ns + "planning_algorithm"); + std::string ns = "goal_planner.pull_over.freespace_parking."; + p.enable_freespace_parking = declare_parameter(ns + "enable_freespace_parking"); + p.freespace_parking_algorithm = + declare_parameter(ns + "freespace_parking_algorithm"); p.freespace_parking_velocity = declare_parameter(ns + "velocity"); p.vehicle_shape_margin = declare_parameter(ns + "vehicle_shape_margin"); - p.common_parameters.time_limit = declare_parameter(ns + "time_limit"); - p.common_parameters.minimum_turning_radius = + p.freespace_parking_common_parameters.time_limit = declare_parameter(ns + "time_limit"); + p.freespace_parking_common_parameters.minimum_turning_radius = declare_parameter(ns + "minimum_turning_radius"); - p.common_parameters.maximum_turning_radius = + p.freespace_parking_common_parameters.maximum_turning_radius = declare_parameter(ns + "maximum_turning_radius"); - p.common_parameters.turning_radius_size = declare_parameter(ns + "turning_radius_size"); - p.common_parameters.maximum_turning_radius = std::max( - p.common_parameters.maximum_turning_radius, p.common_parameters.minimum_turning_radius); - p.common_parameters.turning_radius_size = std::max(p.common_parameters.turning_radius_size, 1); - - p.common_parameters.theta_size = declare_parameter(ns + "theta_size"); - p.common_parameters.angle_goal_range = declare_parameter(ns + "angle_goal_range"); - - p.common_parameters.curve_weight = declare_parameter(ns + "curve_weight"); - p.common_parameters.reverse_weight = declare_parameter(ns + "reverse_weight"); - p.common_parameters.lateral_goal_range = declare_parameter(ns + "lateral_goal_range"); - p.common_parameters.longitudinal_goal_range = + p.freespace_parking_common_parameters.turning_radius_size = + declare_parameter(ns + "turning_radius_size"); + p.freespace_parking_common_parameters.maximum_turning_radius = std::max( + p.freespace_parking_common_parameters.maximum_turning_radius, + p.freespace_parking_common_parameters.minimum_turning_radius); + p.freespace_parking_common_parameters.turning_radius_size = + std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); + } + + // freespace parking search config + { + std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + p.freespace_parking_common_parameters.theta_size = declare_parameter(ns + "theta_size"); + p.freespace_parking_common_parameters.angle_goal_range = + declare_parameter(ns + "angle_goal_range"); + p.freespace_parking_common_parameters.curve_weight = + declare_parameter(ns + "curve_weight"); + p.freespace_parking_common_parameters.reverse_weight = + declare_parameter(ns + "reverse_weight"); + p.freespace_parking_common_parameters.lateral_goal_range = + declare_parameter(ns + "lateral_goal_range"); + p.freespace_parking_common_parameters.longitudinal_goal_range = declare_parameter(ns + "longitudinal_goal_range"); + } - // costmap configs - p.common_parameters.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); + // freespace parking costmap configs + { + std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + p.freespace_parking_common_parameters.obstacle_threshold = + declare_parameter(ns + "obstacle_threshold"); } + // freespace parking astar { - std::string ns = "pull_over.freespace_parking.astar."; + std::string ns = "goal_planner.pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = declare_parameter(ns + "use_back"); @@ -906,8 +981,9 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() declare_parameter(ns + "distance_heuristic_weight"); } + // freespace parking rrtstar { - std::string ns = "pull_over.freespace_parking.rrtstar."; + std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = declare_parameter(ns + "use_informed_sampling"); @@ -916,6 +992,28 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.rrt_star_parameters.margin = declare_parameter(ns + "margin"); } + // debug + { + std::string ns = "goal_planner.debug."; + p.print_debug_info = declare_parameter(ns + "print_debug_info"); + } + + // validation of parameters + if (p.shift_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + get_logger(), "shift_sampling_num must be positive integer. Given parameter: " + << p.shift_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + if (p.maximum_deceleration < 0.0) { + RCLCPP_FATAL_STREAM( + get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + return p; } @@ -943,26 +1041,23 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() // geometric pull out p.enable_geometric_pull_out = declare_parameter(ns + "enable_geometric_pull_out"); p.divide_pull_out_path = declare_parameter(ns + "divide_pull_out_path"); - p.geometric_pull_out_velocity = declare_parameter(ns + "geometric_pull_out_velocity"); - p.arc_path_interval = declare_parameter(ns + "arc_path_interval"); - p.lane_departure_margin = declare_parameter(ns + "lane_departure_margin"); - p.backward_velocity = declare_parameter(ns + "backward_velocity"); - p.pull_out_max_steer_angle = declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.pull_out_velocity = + declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_path_interval = + declare_parameter(ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + declare_parameter(ns + "lane_departure_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg // search start pose backward p.search_priority = declare_parameter( ns + "search_priority"); // "efficient_path" or "short_back_distance" p.enable_back = declare_parameter(ns + "enable_back"); + p.backward_velocity = declare_parameter(ns + "backward_velocity"); p.max_back_distance = declare_parameter(ns + "max_back_distance"); p.backward_search_resolution = declare_parameter(ns + "backward_search_resolution"); p.backward_path_update_duration = declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = declare_parameter(ns + "ignore_distance_from_lane_end"); - // drivable area - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); // validation of parameters if (p.pull_out_sampling_num < 1) { @@ -1159,6 +1254,7 @@ void BehaviorPathPlannerNode::run() if (output.modified_goal) { PoseWithUuidStamped modified_goal = *(output.modified_goal); modified_goal.header.stamp = path->header.stamp; + planner_data_->prev_modified_goal = modified_goal; modified_goal_publisher_->publish(modified_goal); } @@ -1172,7 +1268,8 @@ void BehaviorPathPlannerNode::run() #ifndef USE_OLD_ARCHITECTURE planner_manager_->print(); - planner_manager_->publishDebugMarker(); + planner_manager_->publishMarker(); + planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ #endif @@ -1189,7 +1286,7 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal.command = TurnIndicatorsCommand::DISABLE; hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { - turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info); + turn_signal = planner_data->getTurnSignal(path, output.turn_signal_info); hazard_signal.command = HazardLightsCommand::DISABLE; } turn_signal.stamp = get_clock()->now(); @@ -1197,13 +1294,14 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal_publisher_->publish(turn_signal); hazard_signal_publisher_->publish(hazard_signal); - publish_steering_factor(turn_signal); + publish_steering_factor(planner_data, turn_signal); } -void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) +void BehaviorPathPlannerNode::publish_steering_factor( + const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal) { const auto [intersection_flag, approaching_intersection_flag] = - turn_signal_decider_.getIntersectionTurnSignalFlag(); + planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -1213,7 +1311,7 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman }); const auto [intersection_pose, intersection_distance] = - turn_signal_decider_.getIntersectionPoseAndDistance(); + planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { if (intersection_flag) { return SteeringFactor::TURNING; @@ -1389,47 +1487,20 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( #else const auto module_status_ptr_vec = planner_manager->getSceneModuleStatus(); #endif - if (skipSmoothGoalConnection(module_status_ptr_vec)) { - connected_path = *path; - } else { - connected_path = modifyPathForSmoothGoalConnection(*path, planner_data); - } const auto resampled_path = utils::resamplePathWithSpline( - connected_path, planner_data->parameters.output_path_interval, - keepInputPoints(module_status_ptr_vec)); + *path, planner_data->parameters.output_path_interval, keepInputPoints(module_status_ptr_vec)); return std::make_shared(resampled_path); } -bool BehaviorPathPlannerNode::skipSmoothGoalConnection( - const std::vector> & statuses) const -{ -#ifdef USE_OLD_ARCHITECTURE - const auto target_module = "PullOver"; -#else - const auto target_module = "pull_over"; -#endif - - const auto target_status = ModuleStatus::RUNNING; - - for (auto & status : statuses) { - if (status->is_waiting_approval || status->status == target_status) { - if (target_module == status->module_name) { - return true; - } - } - } - return false; -} - // This is a temporary process until motion planning can take the terminal pose into account bool BehaviorPathPlannerNode::keepInputPoints( const std::vector> & statuses) const { #ifdef USE_OLD_ARCHITECTURE - const std::vector target_modules = {"PullOver", "Avoidance"}; + const std::vector target_modules = {"GoalPlanner", "Avoidance"}; #else - const std::vector target_modules = {"pull_over", "avoidance"}; + const std::vector target_modules = {"goal_planner", "avoidance"}; #endif const auto target_status = ModuleStatus::RUNNING; @@ -1526,17 +1597,31 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( return result; } +#ifndef USE_OLD_ARCHITECTURE + { + const std::lock_guard lock(mutex_manager_); // for planner_manager_ + planner_manager_->updateModuleParams(parameters); + } +#endif + result.successful = true; result.reason = "success"; try { - updateParam( - parameters, "avoidance.publish_debug_marker", avoidance_param_ptr_->publish_debug_marker); updateParam( parameters, "lane_change.publish_debug_marker", lane_change_param_ptr_->publish_debug_marker); // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; const std::lock_guard lock(mutex_pd_); // for planner_data_ + updateParam( + parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM, + planner_data_->drivable_area_expansion_parameters.drivable_area_right_bound_offset); + updateParam( + parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM, + planner_data_->drivable_area_expansion_parameters.drivable_area_left_bound_offset); + updateParam( + parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_TYPES_TO_SKIP_PARAM, + planner_data_->drivable_area_expansion_parameters.drivable_area_types_to_skip); updateParam( parameters, DrivableAreaExpansionParameters::ENABLED_PARAM, planner_data_->drivable_area_expansion_parameters.enabled); @@ -1598,31 +1683,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( return result; } - -PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( - const PathWithLaneId & path, const std::shared_ptr & planner_data) const -{ - const auto goal = planner_data->route_handler->getGoalPose(); - const auto goal_lane_id = planner_data->route_handler->getGoalLaneId(); - - Pose refined_goal{}; - { - lanelet::ConstLanelet goal_lanelet; - if (planner_data->route_handler->getGoalLanelet(&goal_lanelet)) { - refined_goal = utils::refineGoal(goal, goal_lanelet); - } else { - refined_goal = goal; - } - } - - auto refined_path = utils::refinePathForGoal( - planner_data->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal, - goal_lane_id); - refined_path.header.frame_id = "map"; - refined_path.header.stamp = this->now(); - - return refined_path; -} } // namespace behavior_path_planner #include diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index a4214f816a86a..c72f5b89ba36d 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -89,6 +89,18 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr *s = SceneModuleStatus{s->module_name}; }); + const bool is_any_module_running = std::any_of( + scene_modules_.begin(), scene_modules_.end(), + [](const auto & module) { return module->getCurrentStatus() == BT::NodeStatus::RUNNING; }); + + if ( + !is_any_module_running && + utils::isEgoOutOfRoute( + data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler)) { + BehaviorModuleOutput output = utils::createGoalAroundPath(data); + return output; + } + // reset blackboard blackboard_->set("output", BehaviorModuleOutput{}); @@ -101,7 +113,9 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr resetNotRunningModulePathCandidate(); std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { + m->publishInfoMarker(); m->publishDebugMarker(); + m->publishVirtualWall(); if (!m->isExecutionRequested()) { m->onExit(); } diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index 7b5309c1d622a..3d04dcd2f56b3 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -17,6 +17,8 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include + #include #include @@ -55,11 +57,25 @@ MarkerArray createObjectsCubeMarkerArray( const ObjectDataArray & objects, std::string && ns, const Vector3 & scale, const ColorRGBA & color) { + using autoware_auto_perception_msgs::msg::ObjectClassification; + MarkerArray msg; - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, scale, color); + const auto is_small_object = [](const auto & o) { + const auto t = behavior_path_planner::utils::getHighestProbLabel(o.classification); + return t == ObjectClassification::PEDESTRIAN || t == ObjectClassification::BICYCLE || + t == ObjectClassification::MOTORCYCLE || t == ObjectClassification::UNKNOWN; + }; + for (const auto & object : objects) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, scale, color); + + if (is_small_object(object.object)) { + marker.scale = createMarkerScale(0.5, 0.5, 1.5); + marker.type = Marker::CYLINDER; + } + marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; msg.markers.push_back(marker); @@ -68,27 +84,52 @@ MarkerArray createObjectsCubeMarkerArray( return msg; } +MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + for (const auto & object : objects) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + + for (const auto & p : object.envelope_poly.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); + } + + marker.points.push_back(marker.points.front()); + marker.id = uuidToInt32(object.object.object_id); + msg.markers.push_back(marker); + } + + return msg; +} + MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - Marker marker = createDefaultMarker( + auto marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { { - const auto to_stop_factor_distance = std::min(object.to_stop_factor_distance, 1000.0); marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2); string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" << "lateral: " << object.lateral << " [-]\n" - << "stop_factor:" << to_stop_factor_distance << " [m]\n" + << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" << "move_time:" << object.move_time << " [s]\n" << "stop_time:" << object.stop_time << " [s]\n"; marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.5, 0.5, 0.5); + marker.ns = ns; msg.markers.push_back(marker); } @@ -108,6 +149,40 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st return msg; } +MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size() * 4); + + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(1.0, 1.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + + return msg; +} + +MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size() * 4); + + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(1.0, 0.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + + return msg; +} + } // namespace MarkerArray createEgoStatusMarkerArray( @@ -135,29 +210,7 @@ MarkerArray createEgoStatusMarkerArray( { std::ostringstream string_stream; string_stream << "ego_state:"; - switch (data.state) { - case AvoidanceState::NOT_AVOID: - string_stream << "NOT_AVOID"; - break; - case AvoidanceState::AVOID_PATH_NOT_READY: - string_stream << "AVOID_PATH_NOT_READY"; - marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - break; - case AvoidanceState::YIELD: - string_stream << "YIELD"; - marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); - break; - case AvoidanceState::AVOID_PATH_READY: - string_stream << "AVOID_PATH_READY"; - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - break; - case AvoidanceState::AVOID_EXECUTE: - string_stream << "AVOID_EXECUTE"; - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - break; - default: - throw std::domain_error("invalid behavior"); - } + string_stream << magic_enum::enum_name(data.state); marker.text = string_stream.str(); marker.pose.position.z += 2.0; marker.id++; @@ -239,7 +292,7 @@ MarkerArray createAvoidLineMarkerArray( for (const auto & sl : shift_lines_local) { // ROS_ERROR("sl: s = (%f, %f), g = (%f, %f)", sl.start.x, sl.start.y, sl.end.x, sl.end.y); - Marker basic_marker = createDefaultMarker( + auto basic_marker = createDefaultMarker( "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(r, g, b, 0.9)); basic_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); @@ -335,95 +388,54 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st return msg; } -MarkerArray createAvoidableTargetObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) +MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns) { - MarkerArray msg; - msg.markers.reserve(objects.size() * 3); - - appendMarkerArray( - createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(1.0, 1.0, 0.0, 0.8)), - &msg); - - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); - - { - for (const auto & object : objects) { - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_envelope_polygon", 0L, - Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); - - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); - } - - marker.points.push_back(marker.points.front()); - marker.id = uuidToInt32(object.object.object_id); - msg.markers.push_back(marker); - } + ObjectDataArray avoidable; + ObjectDataArray unavoidable; + for (const auto & o : objects) { + if (o.is_avoidable) { + avoidable.push_back(o); + } else { + unavoidable.push_back(o); } } + MarkerArray msg; + msg.markers.reserve(objects.size() * 4); + + appendMarkerArray(avoidableObjectsMarkerArray(avoidable, "avoidable_" + ns), &msg); + appendMarkerArray(unAvoidableObjectsMarkerArray(unavoidable, "unavoidable_" + ns), &msg); + return msg; } -MarkerArray createUnavoidableTargetObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) +MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns) { - MarkerArray msg; - msg.markers.reserve(objects.size() * 3); + using behavior_path_planner::utils::convertToSnakeCase; - appendMarkerArray( - createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(1.0, 0.0, 0.0, 0.8)), - &msg); - - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); - - { - for (const auto & object : objects) { - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_envelope_polygon", 0L, - Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); - - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); - } - - marker.points.push_back(marker.points.front()); - marker.id = uuidToInt32(object.object.object_id); - msg.markers.push_back(marker); + const auto filtered_objects = [&objects, &ns]() { + ObjectDataArray ret{}; + for (const auto & o : objects) { + if (o.reason != ns) { + continue; } + ret.push_back(o); } - } - return msg; -} + return ret; + }(); -MarkerArray createOtherObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) -{ MarkerArray msg; - msg.markers.reserve(objects.size() * 2); + msg.markers.reserve(filtered_objects.size() * 2); appendMarkerArray( createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(0.0, 1.0, 0.0, 0.8)), + filtered_objects, "others_" + convertToSnakeCase(ns) + "_cube", + createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)), + &msg); + appendMarkerArray( + createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"), &msg); - - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); return msg; } @@ -434,24 +446,10 @@ MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std: objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); } -MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) -{ - MarkerArray msg; - - appendMarkerArray( - createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), - createMarkerColor(1.0, 0.0, 1.0, 0.9)), - &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); - - return msg; -} - MarkerArray makeOverhangToRoadShoulderMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) + const ObjectDataArray & objects, std::string && ns) { - Marker marker = createDefaultMarker( + auto marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0)); @@ -478,7 +476,7 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( for (const auto & linestring : linestrings) { const auto id = static_cast(linestring.id()); - Marker marker = createDefaultMarker( + auto marker = createDefaultMarker( "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index dbc3374d27a20..30d712226a1e3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include #include @@ -26,12 +27,8 @@ namespace behavior_path_planner { - -PlannerManager::PlannerManager( - rclcpp::Node & node, const std::shared_ptr & parameters, - const bool verbose) -: parameters_{parameters}, - logger_(node.get_logger().get_child("planner_manager")), +PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) +: logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), verbose_{verbose} { @@ -50,56 +47,121 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); - while (rclcpp::ok()) { - /** - * STEP1: get approved modules' output - */ - const auto approved_modules_output = runApprovedModules(data); + auto result_output = [&]() { + const bool is_any_approved_module_running = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING; }); - /** - * STEP2: check modules that need to be launched - */ - const auto request_modules = getRequestModules(approved_modules_output); + const bool is_any_candidate_module_running = std::any_of( + candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING; }); - /** - * STEP3: if there is no module that need to be launched, return approved modules' output - */ - if (request_modules.empty()) { - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; - } + const bool is_any_module_running = + is_any_approved_module_running || is_any_candidate_module_running; - /** - * STEP4: if there is module that should be launched, execute the module - */ - const auto [highest_priority_module, candidate_modules_output] = - runRequestModules(request_modules, data, approved_modules_output); - if (!highest_priority_module) { - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + const bool is_out_of_route = utils::isEgoOutOfRoute( + data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler); + + if (!is_any_module_running && is_out_of_route) { + BehaviorModuleOutput output = utils::createGoalAroundPath(data); + generateCombinedDrivableArea(output, data); + return output; } - /** - * STEP5: if the candidate module's modification is NOT approved yet, return the result. - * NOTE: the result is output of the candidate module, but the output path don't contains path - * shape modification that needs approval. On the other hand, it could include velocity profile - * modification. - */ - if (highest_priority_module->isWaitingApproval()) { - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + while (rclcpp::ok()) { + /** + * STEP1: get approved modules' output + */ + const auto approved_modules_output = runApprovedModules(data); + + /** + * STEP2: check modules that need to be launched + */ + const auto request_modules = getRequestModules(approved_modules_output); + + /** + * STEP3: if there is no module that need to be launched, return approved modules' output + */ + if (request_modules.empty()) { + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return approved_modules_output; + } + + /** + * STEP4: if there is module that should be launched, execute the module + */ + const auto [highest_priority_module, candidate_modules_output] = + runRequestModules(request_modules, data, approved_modules_output); + if (!highest_priority_module) { + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return approved_modules_output; + } + + /** + * STEP5: if the candidate module's modification is NOT approved yet, return the result. + * NOTE: the result is output of the candidate module, but the output path don't contains path + * shape modification that needs approval. On the other hand, it could include velocity + * profile modification. + */ + if (highest_priority_module->isWaitingApproval()) { + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return candidate_modules_output; + } + + /** + * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ + */ + addApprovedModule(highest_priority_module); + clearCandidateModules(); } + return BehaviorModuleOutput{}; + }(); - /** - * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ - */ - addApprovedModule(highest_priority_module); - clearCandidateModules(); + generateCombinedDrivableArea(result_output, data); + + return result_output; +} + +// NOTE: To deal with some policies about drivable area generation, currently DrivableAreaInfo is +// quite messy. Needs to be refactored. +void PlannerManager::generateCombinedDrivableArea( + BehaviorModuleOutput & output, const std::shared_ptr & data) const +{ + if (output.path->points.empty()) { + RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!"); + return; } - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + const auto & di = output.drivable_area_info; + constexpr double epsilon = 1e-3; + + if (epsilon < std::abs(di.drivable_margin)) { + // for single free space pull over + const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; + + utils::generateDrivableArea( + *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); + } else if (di.is_already_expanded) { + // for single side shift + utils::generateDrivableArea( + *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data); + } else { + const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); + + const auto & dp = data->drivable_area_expansion_parameters; + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + // for other modules where multiple modules may be launched + utils::generateDrivableArea( + *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, + data->parameters.vehicle_length, data); + } - return {}; + // extract obstacles from drivable area + utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles); } std::vector PlannerManager::getRequestModules( @@ -387,12 +449,12 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() != ModuleStatus::SUCCESS; }); + + // convert reverse iterator -> iterator + const auto success_itr = std::prev(not_success_itr).base() - 1; + /** - * 1.remove success modules. these modules' outputs are used as valid plan. - * 2.update root lanelet if lane change module succeeded path planning, and finished correctly. + * there is no succeeded module. return. */ - { - const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::SUCCESS; }); + if (success_itr == approved_module_ptrs_.end()) { + return approved_modules_output; + } - if (itr == approved_module_ptrs_.begin()) { - if ((*itr)->name().find("lane_change") != std::string::npos) { - root_lanelet_ = updateRootLanelet(data); - } + const auto lane_change_itr = std::find_if( + success_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - clearCandidateModules(); - deleteExpiredModules(*itr); - approved_module_ptrs_.erase(itr); - } + /** + * remove success modules according to Last In First Out(LIFO) policy. when the next module is in + * ModuleStatus::RUNNING, the previous module keeps running even if it is in + * ModuleStatus::SUCCESS. + */ + if (lane_change_itr == approved_module_ptrs_.end()) { + std::for_each( + success_itr, approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); }); + + approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); + clearCandidateModules(); + + return approved_modules_output; + } + + /** + * as an exception, when there is lane change module is in succeeded modules, it doesn't remove + * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the + * root lanelet is updated at the moment of lane change module's unregistering, and that causes + * change First In module's input. + */ + if (not_success_itr == approved_module_ptrs_.rend()) { + std::for_each( + success_itr, approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); }); + + approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); + clearCandidateModules(); + + root_lanelet_ = updateRootLanelet(data); + + return approved_modules_output; } return approved_modules_output; @@ -547,6 +640,10 @@ void PlannerManager::print() const return; } + const auto get_status = [](const auto & m) { + return magic_enum::enum_name(m->getCurrentStatus()); + }; + size_t max_string_num = 0; std::ostringstream string_stream; @@ -563,13 +660,15 @@ void PlannerManager::print() const string_stream << "\n"; string_stream << "approved modules : "; for (const auto & m : approved_module_ptrs_) { - string_stream << "[" << m->name() << "]->"; + string_stream << "[" << m->name() << "(" << get_status(m) << ")" + << "]->"; } string_stream << "\n"; string_stream << "candidate module : "; for (const auto & m : candidate_module_ptrs_) { - string_stream << "[" << m->name() << "]->"; + string_stream << "[" << m->name() << "(" << get_status(m) << ")" + << "]->"; } string_stream << "\n" << std::fixed << std::setprecision(1); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index da30de58bc968..0d9b20317c993 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -16,7 +16,7 @@ #include "behavior_path_planner/marker_util/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/avoidance/util.hpp" +#include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -53,7 +53,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; using tier4_autoware_utils::calcLongitudinalDeviation; -using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; @@ -97,15 +96,22 @@ bool AvoidanceModule::isExecutionRequested() const { DEBUG_PRINT("AVOIDANCE isExecutionRequested"); +#ifndef USE_OLD_ARCHITECTURE + const auto is_driving_forward = + motion_utils::isDrivingForward(getPreviousModuleOutput().path->points); + if (!is_driving_forward || !(*is_driving_forward)) { + return false; + } +#endif + if (current_state_ == ModuleStatus::RUNNING) { return true; } - // Check avoidance targets exist - auto avoid_data = calcAvoidancePlanningData(debug_data_); - // Check ego is in preferred lane #ifdef USE_OLD_ARCHITECTURE + const auto avoid_data = calcAvoidancePlanningData(debug_data_); + const auto current_lanes = utils::getCurrentLanes(planner_data_); lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet( @@ -116,33 +122,29 @@ bool AvoidanceModule::isExecutionRequested() const return false; } #else - fillShiftLine(avoid_data, debug_data_); + const auto avoid_data = avoidance_data_; +#endif + + updateInfoMarker(avoid_data); + updateDebugMarker(avoid_data, path_shifter_, debug_data_); + +#ifndef USE_OLD_ARCHITECTURE + // there is object that should be avoid. return true. + if (!!avoid_data.stop_target_object) { + return true; + } if (avoid_data.unapproved_new_sl.empty()) { return false; } #endif - if (parameters_->publish_debug_marker) { - setDebugData(avoid_data, path_shifter_, debug_data_); - } - return !avoid_data.target_objects.empty(); } bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - - { - DebugData debug; - static_cast(calcAvoidancePlanningData(debug)); - } - - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - return true; } @@ -151,21 +153,21 @@ ModuleStatus AvoidanceModule::updateState() const auto is_plan_running = isAvoidancePlanRunning(); const bool has_avoidance_target = !avoidance_data_.target_objects.empty(); + DEBUG_PRINT( + "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + if (!is_plan_running && !has_avoidance_target) { - current_state_ = ModuleStatus::SUCCESS; - } else if ( + return ModuleStatus::SUCCESS; + } + + if ( !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && !isAvoidanceManeuverRunning()) { // if dynamic objects are removed on path, change current state to reset path - current_state_ = ModuleStatus::SUCCESS; - } else { - current_state_ = ModuleStatus::RUNNING; + return ModuleStatus::SUCCESS; } - DEBUG_PRINT( - "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); - - return current_state_; + return ModuleStatus::RUNNING; } bool AvoidanceModule::isAvoidancePlanRunning() const @@ -193,11 +195,31 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb AvoidancePlanningData data; // reference pose - const auto reference_pose = getUnshiftedEgoPose(prev_output_); + const auto reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); data.reference_pose = reference_pose; + // special for avoidance: take behind distance upt ot shift-start-point if it exist. + const auto longest_dist_to_shift_line = [&]() { + double max_dist = 0.0; + for (const auto & pnt : path_shifter_.getShiftLines()) { + max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); + } + for (const auto & sl : registered_raw_shift_lines_) { + max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sl.start)); + } + return max_dist; + }(); + // center line path (output of this function must have size > 1) - const auto center_path = calcCenterLinePath(planner_data_, reference_pose); +#ifdef USE_OLD_ARCHITECTURE + const auto center_path = + utils::calcCenterLinePath(planner_data_, reference_pose, longest_dist_to_shift_line); +#else + const auto center_path = utils::calcCenterLinePath( + planner_data_, reference_pose, longest_dist_to_shift_line, + *getPreviousModuleOutput().reference_path); +#endif + debug.center_line = center_path; if (center_path.points.size() < 2) { RCLCPP_WARN_THROTTLE( @@ -207,14 +229,14 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // reference path #ifdef USE_OLD_ARCHITECTURE - data.reference_path = - utils::resamplePathWithSpline(center_path, parameters_->resample_interval_for_planning); + data.reference_path_rough = center_path; #else - const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path); - data.reference_path = utils::resamplePathWithSpline( - backward_extened_path, parameters_->resample_interval_for_planning); + data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); #endif + data.reference_path = utils::resamplePathWithSpline( + data.reference_path_rough, parameters_->resample_interval_for_planning); + if (data.reference_path.points.size() < 2) { // if the resampled path has only 1 point, use original path. data.reference_path = center_path; @@ -254,7 +276,7 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - const auto expanded_lanelets = getTargetLanelets( + const auto expanded_lanelets = utils::avoidance::getTargetLanelets( planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, parameters_->detection_area_right_expand_dist * (-1.0)); @@ -273,7 +295,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( objects.push_back(createObjectData(data, object)); } - filterTargetObjects(objects, data, debug, planner_data_, parameters_); + utils::avoidance::filterTargetObjects(objects, data, debug, planner_data_, parameters_); // debug { @@ -314,31 +336,31 @@ ObjectData AvoidanceModule::createObjectData( object_data.object = object; // Calc envelop polygon. - fillObjectEnvelopePolygon(object_data, registered_objects_, object_closest_pose, parameters_); + utils::avoidance::fillObjectEnvelopePolygon( + object_data, registered_objects_, object_closest_pose, parameters_); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); - // calc longitudinal distance from ego to closest target object footprint point. - fillLongitudinalAndLengthByClosestEnvelopeFootprint( - data.reference_path, getEgoPosition(), object_data); - // Calc moving time. - fillObjectMovingTime(object_data, stopped_objects_, parameters_); + utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); // Calc lateral deviation from path to target object. object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = calcEnvelopeOverhangDistance( + object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( object_data, object_closest_pose, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. + const auto t = utils::getHighestProbLabel(object.classification); + const auto object_parameter = parameters_->object_parameters.at(t); const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto safety_margin = 0.5 * vehicle_width + parameters_->lateral_passable_safety_buffer; + const auto safety_margin = 0.5 * vehicle_width + object_parameter.safety_buffer_lateral; object_data.avoid_required = - (isOnRight(object_data) && std::abs(object_data.overhang_dist) < safety_margin) || - (!isOnRight(object_data) && object_data.overhang_dist < safety_margin); + (utils::avoidance::isOnRight(object_data) && + std::abs(object_data.overhang_dist) < safety_margin) || + (!utils::avoidance::isOnRight(object_data) && object_data.overhang_dist < safety_margin); return object_data; } @@ -421,7 +443,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ if (!data.safe && data.avoid_required) { data.yield_required = data.found_avoidance_path && data.avoid_required; - data.candidate_path = toShiftedPath(data.reference_path); + data.candidate_path = utils::avoidance::toShiftedPath(data.reference_path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "not found safe avoidance path. transit yield maneuver..."); } @@ -432,7 +454,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ if (!data.safe && registered) { data.yield_required = true; - data.candidate_path = toShiftedPath(data.reference_path); + data.candidate_path = utils::avoidance::toShiftedPath(data.reference_path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "found safe avoidance path, but it is not safe. canceling avoidance path..."); @@ -481,19 +503,17 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat const auto max_avoid_margin = object_parameter.safety_buffer_lateral + parameters_->lateral_collision_margin + 0.5 * vehicle_width; - const auto variable = - getSharpAvoidanceDistance(getShiftLength(o_front, isOnRight(o_front), max_avoid_margin)); + const auto variable = getSharpAvoidanceDistance( + getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); const auto constant = getNominalPrepareDistance() + object_parameter.safety_buffer_longitudinal + base_link2front; const auto total_avoid_distance = variable + constant; - const auto opt_feasible_bound = calcLongitudinalOffsetPose( + dead_pose_ = calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); - if (opt_feasible_bound) { - debug.feasible_bound = opt_feasible_bound.get(); - } else { - debug.feasible_bound = getPose(data.reference_path.points.front()); + if (!dead_pose_) { + dead_pose_ = getPose(data.reference_path.points.front()); } } @@ -684,7 +704,7 @@ double AvoidanceModule::getShiftLength( const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const { const auto shift_length = - behavior_path_planner::calcShiftLength(is_object_on_right, object.overhang_dist, avoid_margin); + utils::avoidance::calcShiftLength(is_object_on_right, object.overhang_dist, avoid_margin); return is_object_on_right ? std::min(shift_length, getLeftShiftBound()) : std::max(shift_length, getRightShiftBound()); } @@ -738,9 +758,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } } - const auto is_object_on_right = isOnRight(o); + const auto is_object_on_right = utils::avoidance::isOnRight(o); const auto shift_length = getShiftLength(o, is_object_on_right, o.avoid_margin.get()); - if (isSameDirectionShift(is_object_on_right, shift_length)) { + if (utils::avoidance::isSameDirectionShift(is_object_on_right, shift_length)) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; debug.unavoidable_objects.push_back(o); @@ -950,9 +970,11 @@ void AvoidanceModule::fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_ calcSignedArcLength(path.points, 0, avoidance_data_.ego_closest_path_index); for (auto & sl : shift_lines) { - sl.start_idx = findPathIndexFromArclength(arclength, sl.start_longitudinal + path_front_to_ego); + sl.start_idx = utils::avoidance::findPathIndexFromArclength( + arclength, sl.start_longitudinal + path_front_to_ego); sl.start = path.points.at(sl.start_idx).point.pose; - sl.end_idx = findPathIndexFromArclength(arclength, sl.end_longitudinal + path_front_to_ego); + sl.end_idx = utils::avoidance::findPathIndexFromArclength( + arclength, sl.end_longitudinal + path_front_to_ego); sl.end = path.points.at(sl.end_idx).point.pose; } } @@ -1027,7 +1049,7 @@ void AvoidanceModule::generateTotalShiftLine( const auto & al = avoid_lines.at(j); for (size_t i = 0; i < N; ++i) { // calc current interpolated shift - const auto i_shift = lerpShiftLengthOnArc(arcs.at(i), al); + const auto i_shift = utils::avoidance::lerpShiftLengthOnArc(arcs.at(i), al); // update maximum shift for positive direction if (i_shift > sl.pos_shift_line.at(i)) { @@ -1127,7 +1149,8 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ // If the vehicle is already on the avoidance (checked by the first point has shift), // set a start point at the first path point. if (!found_first_start && std::abs(shift) > IS_ALREADY_SHIFTING_THR) { - setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. + utils::avoidance::setStartData( + al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. found_first_start = true; DEBUG_PRINT("shift (= %f) is not zero at i = %lu. set start shift here.", shift, i); } @@ -1141,14 +1164,16 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ } if (!found_first_start) { - setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. + utils::avoidance::setStartData( + al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. found_first_start = true; DEBUG_PRINT("grad change detected. start at i = %lu", i); } else { - setEndData(al, shift, p, i, arcs.at(i)); + utils::avoidance::setEndData(al, shift, p, i, arcs.at(i)); al.id = getOriginalShiftLineUniqueId(); merged_avoid_lines.push_back(al); - setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. + utils::avoidance::setStartData( + al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. DEBUG_PRINT("end and start point found at i = %lu", i); } } @@ -1265,8 +1290,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Combine avoid points that have almost same gradient. // this is to remove the noise. { - const auto CHANGE_SHIFT_THRESHOLD_FOR_NOISE = 0.1; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD_FOR_NOISE); + const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift"); } @@ -1274,8 +1299,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { - constexpr double QUANTIZATION_DISTANCE = 0.2; - quantizeShiftLine(sl_array_trimmed, QUANTIZATION_DISTANCE); + const auto THRESHOLD = parameters_->quantize_filter_threshold; + quantizeShiftLine(sl_array_trimmed, THRESHOLD); printShiftLines(sl_array_trimmed, "after sl_array_trimmed"); debug.quantized = sl_array_trimmed; } @@ -1290,8 +1315,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Combine avoid points that have almost same gradient (again) { - const auto CHANGE_SHIFT_THRESHOLD = 0.2; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD); + const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift_second = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); } @@ -1300,15 +1325,16 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // Check if it is not too sharp for the return-to-center shift point. // If the shift is sharp, it is combined with the next shift point until it gets non-sharp. { - trimSharpReturn(sl_array_trimmed); + const auto THRESHOLD = parameters_->sharp_shift_filter_threshold; + trimSharpReturn(sl_array_trimmed, THRESHOLD); debug.trim_too_sharp_shift = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trimSharpReturn"); } // - Combine avoid points that have almost same gradient (again) { - const auto CHANGE_SHIFT_THRESHOLD = 0.2; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD); + const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift_third = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); } @@ -1339,21 +1365,20 @@ void AvoidanceModule::alignShiftLinesOrder( } } -void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const +void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const { - if (interval < 1.0e-5) { + if (threshold < 1.0e-5) { return; // no need to process } for (auto & sl : shift_lines) { - sl.end_shift_length = std::round(sl.end_shift_length / interval) * interval; + sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold; } alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::trimSmallShiftLine( - AvoidLineArray & shift_lines, const double shift_diff_thres) const +void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1368,7 +1393,7 @@ void AvoidanceModule::trimSmallShiftLine( auto sl_modified = sl_now; // remove the shift point if the length is almost same as the previous one. - if (std::abs(shift_diff) < shift_diff_thres) { + if (std::abs(shift_diff) < threshold) { sl_modified.end_shift_length = sl_prev.end_shift_length; sl_modified.start_shift_length = sl_prev.end_shift_length; DEBUG_PRINT( @@ -1386,7 +1411,7 @@ void AvoidanceModule::trimSmallShiftLine( } void AvoidanceModule::trimSimilarGradShiftLine( - AvoidLineArray & avoid_lines, const double change_shift_dist_threshold) const + AvoidLineArray & avoid_lines, const double threshold) const { AvoidLineArray avoid_lines_orig = avoid_lines; avoid_lines.clear(); @@ -1403,17 +1428,17 @@ void AvoidanceModule::trimSimilarGradShiftLine( being_merged_points.push_back(al_prev); // This point is about to be merged. auto combined_al = al_prev; - setEndData( + utils::avoidance::setEndData( combined_al, al_now.end_shift_length, al_now.end, al_now.end_idx, al_now.end_longitudinal); - combined_al.parent_ids = concatParentIds(combined_al.parent_ids, al_prev.parent_ids); + combined_al.parent_ids = + utils::avoidance::concatParentIds(combined_al.parent_ids, al_prev.parent_ids); const auto has_large_length_change = [&]() { for (const auto & original : being_merged_points) { const auto longitudinal = original.end_longitudinal - combined_al.start_longitudinal; const auto new_length = combined_al.getGradient() * longitudinal + combined_al.start_shift_length; - const bool has_large_change = - std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold; + const bool has_large_change = std::abs(new_length - original.end_shift_length) > threshold; DEBUG_PRINT( "original.end_shift_length: %f, original.end_longitudinal: %f, " @@ -1422,7 +1447,7 @@ void AvoidanceModule::trimSimilarGradShiftLine( original.end_shift_length, original.end_longitudinal, combined_al.start_longitudinal, combined_al.getGradient(), new_length, has_large_change); - if (std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold) { + if (std::abs(new_length - original.end_shift_length) > threshold) { return true; } } @@ -1513,10 +1538,11 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const "next shift).", i, sl_next.getRelativeLength()); auto sl_modified = sl_next; - setStartData( + utils::avoidance::setStartData( sl_modified, sl_now.end_shift_length, sl_now.start, sl_now.start_idx, sl_now.start_longitudinal); - sl_modified.parent_ids = concatParentIds(sl_modified.parent_ids, sl_now.parent_ids); + sl_modified.parent_ids = + utils::avoidance::concatParentIds(sl_modified.parent_ids, sl_now.parent_ids); shift_lines.push_back(sl_modified); ++i; // skip next shift point continue; @@ -1564,7 +1590,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const sl_next_modified.start_longitudinal = std::max(sl_next_avoid.end_longitudinal - avoid_distance, sl_now.start_longitudinal); sl_next_modified.start_idx = - findPathIndexFromArclength(arclength, sl_next_modified.start_longitudinal); + utils::avoidance::findPathIndexFromArclength(arclength, sl_next_modified.start_longitudinal); sl_next_modified.start = avoidance_data_.reference_path.points.at(sl_next_modified.start_idx).point.pose; sl_next_modified.parent_ids = calcParentIds(current_raw_shift_lines_, sl_next_modified); @@ -1573,7 +1599,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const if (sl_next_modified.start_idx > sl_now.start_idx) { // the case where a straight route exists. auto sl_now_modified = sl_now; sl_now_modified.start_shift_length = sl_prev_length; - setEndData( + utils::avoidance::setEndData( sl_now_modified, sl_prev_length, sl_next_modified.start, sl_next_modified.start_idx, sl_next_modified.start_longitudinal); sl_now_modified.parent_ids = calcParentIds(current_raw_shift_lines_, sl_now_modified); @@ -1595,7 +1621,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const DEBUG_PRINT("trimMomentaryReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } -void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const +void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1617,24 +1643,23 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const // combine two shift points. Be careful the order of "now" and "next". const auto combineShiftLine = [this](const auto & sl_next, const auto & sl_now) { auto sl_modified = sl_now; - setEndData( + utils::avoidance::setEndData( sl_modified, sl_next.end_shift_length, sl_next.end, sl_next.end_idx, sl_next.end_longitudinal); - sl_modified.parent_ids = concatParentIds(sl_modified.parent_ids, sl_now.parent_ids); + sl_modified.parent_ids = + utils::avoidance::concatParentIds(sl_modified.parent_ids, sl_now.parent_ids); return sl_modified; }; // Check if the merged shift has a conflict with the original shifts. - const auto hasViolation = [this](const auto & combined, const auto & combined_src) { - constexpr auto VIOLATION_SHIFT_THR = 0.3; + const auto hasViolation = [&threshold](const auto & combined, const auto & combined_src) { for (const auto & sl : combined_src) { - const auto combined_shift = lerpShiftLengthOnArc(sl.end_longitudinal, combined); - if ( - sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + VIOLATION_SHIFT_THR) { + const auto combined_shift = + utils::avoidance::lerpShiftLengthOnArc(sl.end_longitudinal, combined); + if (sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + threshold) { return true; } - if ( - sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - VIOLATION_SHIFT_THR) { + if (sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - threshold) { return true; } } @@ -1765,7 +1790,7 @@ void AvoidanceModule::trimTooSharpShift(AvoidLineArray & avoid_lines) const // The avoidance_point_now exceeds jerk limit, so merge it with the next avoidance_point. for (size_t j = i + 1; j < avoid_lines_orig.size(); ++j) { auto al_next = avoid_lines_orig.at(j); - setEndData( + utils::avoidance::setEndData( al_now, al_next.end_shift_length, al_next.end, al_next.end_idx, al_next.end_longitudinal); if (isInJerkLimit(al_now)) { avoid_lines.push_back(al_now); @@ -1839,8 +1864,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; + last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = getCurrentBaseShift(); } } @@ -1875,8 +1900,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; + last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = current_base_shift; } @@ -1943,7 +1968,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo( al.start_idx = last_sl.end_idx; al.start = last_sl.end; al.start_longitudinal = arclength_from_ego.at(al.start_idx); - al.end_idx = findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.end_idx = + utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; @@ -1960,10 +1986,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo( { AvoidLine al; al.id = getOriginalShiftLineUniqueId(); - al.start_idx = findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.start_idx = + utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.start = avoidance_data_.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = arclength_from_ego.at(al.start_idx); - al.end_idx = findPathIndexFromArclength( + al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); @@ -1971,7 +1998,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( al.start_shift_length = last_sl.end_shift_length; sl_candidates.push_back(al); printShiftLines(AvoidLineArray{al}, "return point"); - debug_data_.extra_return_shift = AvoidLineArray{al}; + debug_data_.extra_return_shift.push_back(al); // TODO(Horibe) think how to store the current object current_raw_shift_lines.push_back(al); @@ -2087,10 +2114,10 @@ bool AvoidanceModule::isEnoughMargin( }(); const auto & v_ego = p_ego.point.longitudinal_velocity_mps; - const auto & v_ego_lon = getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); + const auto & v_ego_lon = utils::avoidance::getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - if (!isTargetObjectType(object.object, parameters_)) { + if (!utils::avoidance::isTargetObjectType(object.object, parameters_)) { return true; } @@ -2143,7 +2170,7 @@ bool AvoidanceModule::isEnoughMargin( return true; } - const auto v_obj_lon = getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); + const auto v_obj_lon = utils::avoidance::getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); double hysteresis_factor = 1.0; if (avoidance_data_.state == AvoidanceState::YIELD) { @@ -2340,7 +2367,7 @@ ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( { ObjectDataArray objects; for (const auto & o : avoidance_data_.other_objects) { - if (isCentroidWithinLanelets(o.object, adjacent_lanes)) { + if (utils::avoidance::isCentroidWithinLanelets(o.object, adjacent_lanes)) { objects.push_back(o); } } @@ -2355,8 +2382,6 @@ ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( // two lanes since which way to avoid is not obvious void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const { - auto & path = *output.path; - const auto has_same_lane = [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { if (lanes.empty()) return false; @@ -2367,6 +2392,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output const auto & route_handler = planner_data_->route_handler; const auto & current_lanes = avoidance_data_.current_lanelets; const auto & enable_opposite = parameters_->enable_avoidance_over_opposite_direction; + std::vector drivable_lanes; for (const auto & current_lane : current_lanes) { DrivableLanes current_drivable_lanes; @@ -2374,7 +2400,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output current_drivable_lanes.right_lane = current_lane; if (!parameters_->enable_avoidance_over_same_direction) { - output.drivable_lanes.push_back(current_drivable_lanes); + drivable_lanes.push_back(current_drivable_lanes); continue; } @@ -2495,20 +2521,29 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output current_drivable_lanes.middle_lanes.push_back(current_lane); } - output.drivable_lanes.push_back(current_drivable_lanes); + drivable_lanes.push_back(current_drivable_lanes); } - const auto shorten_lanes = utils::cutOverlappedLanes(path, output.drivable_lanes); + { // for new architecture + DrivableAreaInfo current_drivable_area_info; + // generate drivable lanes + current_drivable_area_info.drivable_lanes = drivable_lanes; + // generate obstacle polygons + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + // expand hatched road markings + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; - const auto extended_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } - { - const auto & p = planner_data_->parameters; - generateDrivableArea( - path, output.drivable_lanes, planner_data_, parameters_, avoidance_data_.target_objects, - p.vehicle_length, parameters_->enable_bound_clipping, parameters_->disable_path_update); + { // for old architecture + // NOTE: Obstacles to avoid are not extracted from the drivable area with an old architecture. + utils::generateDrivableArea( + *output.path, drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); } } @@ -2550,6 +2585,11 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted *ego_velocity_starting_avoidance_ptr_ = getEgoSpeed(); } + // update ego velocity if the ego is faster than saved velocity. + if (*ego_velocity_starting_avoidance_ptr_ < getEgoSpeed()) { + *ego_velocity_starting_avoidance_ptr_ = getEgoSpeed(); + } + // calc index and velocity to NO_ACCEL_TIME_THR const auto v0 = *ego_velocity_starting_avoidance_ptr_; auto vmax = 0.0; @@ -2626,61 +2666,6 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig return extended_path; } -// TODO(Horibe) clean up functions: there is a similar code in util as well. -PathWithLaneId AvoidanceModule::calcCenterLinePath( - const std::shared_ptr & planner_data, const Pose & pose) const -{ - const auto & p = planner_data->parameters; - const auto & route_handler = planner_data->route_handler; - - PathWithLaneId centerline_path; - - // special for avoidance: take behind distance upt ot shift-start-point if it exist. - const auto longest_dist_to_shift_line = [&]() { - double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); - } - for (const auto & sl : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sl.start)); - } - return max_dist; - }(); - - printShiftLines(path_shifter_.getShiftLines(), "path_shifter_.getShiftLines()"); - printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); - - const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. - const auto backward_length = - std::max(p.backward_path_length, longest_dist_to_shift_line + extra_margin); - - DEBUG_PRINT( - "p.backward_path_length = %f, longest_dist_to_shift_line = %f, backward_length = %f", - p.backward_path_length, longest_dist_to_shift_line, backward_length); - -#ifdef USE_OLD_ARCHITECTURE - const lanelet::ConstLanelets current_lanes = - utils::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); -#else - const lanelet::ConstLanelets current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - centerline_path = utils::getCenterLinePath( - *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); - - // for debug: check if the path backward distance is same as the desired length. - // { - // const auto back_to_ego = motion_utils::calcSignedArcLength( - // centerline_path.points, centerline_path.points.front().point.pose.position, - // getEgoPosition()); - // RCLCPP_INFO(getLogger(), "actual back_to_ego distance = %f", back_to_ego); - // } - - centerline_path.header = route_handler->getRouteHeader(); - - return centerline_path; -} - boost::optional AvoidanceModule::calcIntersectionShiftLine( const AvoidancePlanningData & data) const { @@ -2739,7 +2724,7 @@ boost::optional AvoidanceModule::calcIntersectionShiftLine( std::abs(obj.longitudinal - ego_to_intersection_dist) > intersection_obstacle_check_dist) { continue; } - if (isOnRight(obj)) { + if (utils::avoidance::isOnRight(obj)) { continue; // TODO(Horibe) Now only think about the left side obstacle. } shift_length = std::min(shift_length, obj.overhang_dist - intersection_shift_margin); @@ -2806,13 +2791,24 @@ BehaviorModuleOutput AvoidanceModule::plan() { postProcess(path_shifter_); // remove old shift points prev_output_ = avoidance_path; - prev_linear_shift_path_ = toShiftedPath(avoidance_data_.reference_path); + prev_linear_shift_path_ = utils::avoidance::toShiftedPath(avoidance_data_.reference_path); path_shifter_.generate(&prev_linear_shift_path_, true, SHIFT_TYPE::LINEAR); prev_reference_ = avoidance_data_.reference_path; } BehaviorModuleOutput output; - output.turn_signal_info = calcTurnSignalInfo(avoidance_path); + + // turn signal info + { + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto new_signal = calcTurnSignalInfo(avoidance_path); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(avoidance_path.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + avoidance_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + } + // sparse resampling for computational cost { avoidance_path.path = @@ -2824,11 +2820,8 @@ BehaviorModuleOutput AvoidanceModule::plan() updateEgoBehavior(data, avoidance_path); } - if (parameters_->publish_debug_marker) { - setDebugData(avoidance_data_, path_shifter_, debug_data_); - } else { - debug_marker_.markers.clear(); - } + updateInfoMarker(avoidance_data_); + updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); output.path = std::make_shared(avoidance_path.path); output.reference_path = getPreviousModuleOutput().reference_path; @@ -2891,24 +2884,35 @@ CandidateOutput AvoidanceModule::planCandidate() const BehaviorModuleOutput AvoidanceModule::planWaitingApproval() { + const auto & data = avoidance_data_; + // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); + #ifndef USE_OLD_ARCHITECTURE if (path_shifter_.getShiftLines().empty()) { out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } #endif + + const auto all_unavoidable = std::all_of( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return !o.is_avoidable; }); + const auto candidate = planCandidate(); - constexpr double threshold_to_update_status = -1.0e-03; - if (candidate.start_distance_to_path_change > threshold_to_update_status) { + if (!avoidance_data_.safe_new_sl.empty()) { updateCandidateRTCStatus(candidate); waitApproval(); + } else if (all_unavoidable) { + waitApproval(); } else { clearWaitingApproval(); removeCandidateRTCStatus(); } + path_candidate_ = std::make_shared(candidate.path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; + return out; } @@ -2953,7 +2957,7 @@ void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) void AvoidanceModule::addNewShiftLines( PathShifter & path_shifter, const AvoidLineArray & new_shift_lines) const { - ShiftLineArray future = toShiftLineArray(new_shift_lines); + ShiftLineArray future = utils::avoidance::toShiftLineArray(new_shift_lines); size_t min_start_idx = std::numeric_limits::max(); for (const auto & sl : new_shift_lines) { @@ -3066,7 +3070,7 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( // DEBUG_PRINT("%s, shift current: %f, candidate: %f", pfx, current_shift, // candidate.end_shift_length); - const auto new_point_threshold = parameters_->avoidance_execution_lateral_threshold; + const auto new_point_threshold = parameters_->lateral_execution_threshold; if (std::abs(candidate.end_shift_length - current_shift) > new_point_threshold) { if (calcJerk(candidate) > parameters_->max_lateral_jerk) { DEBUG_PRINT( @@ -3085,27 +3089,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( return {}; } -Pose AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const -{ - const auto ego_pose = getEgoPose(); - - if (prev_path.path.points.empty()) { - return ego_pose; - } - - // un-shifted fot current ideal pose - const auto closest = findNearestIndex(prev_path.path.points, ego_pose.position); - - // NOTE: Considering avoidance by motion, we set unshifted_pose as previous path instead of - // ego_pose. - Pose unshifted_pose = motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; - - unshifted_pose = calcOffsetPose(unshifted_pose, 0.0, -prev_path.shift_length.at(closest), 0.0); - unshifted_pose.orientation = ego_pose.orientation; - - return unshifted_pose; -} - ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) const { DEBUG_PRINT("path_shifter: base shift = %f", getCurrentBaseShift()); @@ -3141,8 +3124,9 @@ void AvoidanceModule::updateData() debug_data_ = DebugData(); avoidance_data_ = calcAvoidancePlanningData(debug_data_); - updateRegisteredObject(registered_objects_, avoidance_data_.target_objects, parameters_); - compensateDetectionLost( + utils::avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, parameters_); + utils::avoidance::compensateDetectionLost( registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); std::sort( @@ -3176,6 +3160,7 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); + waitApproval(); } void AvoidanceModule::processOnExit() @@ -3286,7 +3271,16 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con return turn_signal_info; } -void AvoidanceModule::setDebugData( +void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const +{ + using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; + + info_marker_.markers.clear(); + appendMarkerArray( + createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); +} + +void AvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { using marker_utils::createLaneletsAreaMarkerArray; @@ -3295,30 +3289,25 @@ void AvoidanceModule::setDebugData( using marker_utils::createPoseMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; - using marker_utils::avoidance_marker::createAvoidableTargetObjectsMarkerArray; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; using marker_utils::avoidance_marker::createSafetyCheckMarkerArray; - using marker_utils::avoidance_marker::createUnavoidableObjectsMarkerArray; - using marker_utils::avoidance_marker::createUnavoidableTargetObjectsMarkerArray; using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; - using motion_utils::createDeadLineVirtualWallMarker; - using motion_utils::createSlowDownVirtualWallMarker; - using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; - using tier4_autoware_utils::calcOffsetPose; debug_marker_.markers.clear(); - const auto & base_link2front = planner_data_->parameters.base_link2front; + + if (!parameters_->publish_debug_marker) { + return; + } + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; + const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); }; const auto addAvoidLine = [&](const AvoidLineArray & al_arr, const auto & ns, auto r, auto g, auto b, double w = 0.1) { @@ -3340,48 +3329,29 @@ void AvoidanceModule::setDebugData( add(createPathMarkerArray(prev_linear_shift_path_.path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - if (debug.stop_pose) { - const auto p_front = calcOffsetPose(debug.stop_pose.get(), base_link2front, 0.0, 0.0); - appendMarkerArray( - createStopVirtualWallMarker(p_front, "avoidance stop", current_time, 0L), &debug_marker_); - } - - if (debug.slow_pose) { - const auto p_front = calcOffsetPose(debug.slow_pose.get(), base_link2front, 0.0, 0.0); - appendMarkerArray( - createSlowDownVirtualWallMarker(p_front, "avoidance slow", current_time, 0L), &debug_marker_); - } - - if (debug.feasible_bound) { - const auto p_front = calcOffsetPose(debug.feasible_bound.get(), base_link2front, 0.0, 0.0); - appendMarkerArray( - createDeadLineVirtualWallMarker(p_front, "feasible bound", current_time, 0L), &debug_marker_); - } - add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); - std::vector avoidable_target_objects; - std::vector unavoidable_target_objects; - for (const auto & object : data.target_objects) { - if (object.is_avoidable) { - avoidable_target_objects.push_back(object); - } else { - unavoidable_target_objects.push_back(object); - } - } - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); - add( - createAvoidableTargetObjectsMarkerArray(avoidable_target_objects, "avoidable_target_objects")); - add(createUnavoidableTargetObjectsMarkerArray( - unavoidable_target_objects, "unavoidable_target_objects")); - add(createOtherObjectsMarkerArray(data.other_objects, "other_objects")); + + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD)); + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL)); + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE)); + add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); + add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); + add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray( - debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); + add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); - add(createUnavoidableObjectsMarkerArray(debug.unavoidable_objects, "unavoidable_objects")); add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); // parent object info @@ -3497,8 +3467,8 @@ void AvoidanceModule::insertWaitPoint( const auto avoid_margin = object_parameter.safety_buffer_lateral + p->lateral_collision_margin + 0.5 * vehicle_width; - const auto variable = - getMinimumAvoidanceDistance(getShiftLength(o_front, isOnRight(o_front), avoid_margin)); + const auto variable = getMinimumAvoidanceDistance( + getShiftLength(o_front, utils::avoidance::isOnRight(o_front), avoid_margin)); const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + base_link2front; const auto start_longitudinal = @@ -3506,16 +3476,16 @@ void AvoidanceModule::insertWaitPoint( std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); if (!use_constraints_for_decel) { - insertDecelPoint( - getEgoPosition(), start_longitudinal, 0.0, shifted_path.path, debug_data_.stop_pose); + utils::avoidance::insertDecelPoint( + getEgoPosition(), start_longitudinal, 0.0, shifted_path.path, stop_pose_); return; } const auto stop_distance = getMildDecelDistance(0.0); if (stop_distance) { const auto insert_distance = std::max(start_longitudinal, *stop_distance); - insertDecelPoint( - getEgoPosition(), insert_distance, 0.0, shifted_path.path, debug_data_.stop_pose); + utils::avoidance::insertDecelPoint( + getEgoPosition(), insert_distance, 0.0, shifted_path.path, stop_pose_); } } @@ -3534,9 +3504,8 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const const auto decel_distance = getMildDecelDistance(p->yield_velocity); if (decel_distance) { - insertDecelPoint( - getEgoPosition(), *decel_distance, p->yield_velocity, shifted_path.path, - debug_data_.slow_pose); + utils::avoidance::insertDecelPoint( + getEgoPosition(), *decel_distance, p->yield_velocity, shifted_path.path, slow_pose_); } } @@ -3564,8 +3533,8 @@ void AvoidanceModule::insertPrepareVelocity(const bool avoidable, ShiftedPath & const auto decel_distance = getFeasibleDecelDistance(0.0); if (decel_distance) { - insertDecelPoint( - getEgoPosition(), *decel_distance, 0.0, shifted_path.path, debug_data_.slow_pose); + utils::avoidance::insertDecelPoint( + getEgoPosition(), *decel_distance, 0.0, shifted_path.path, slow_pose_); } } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index fba08bd39c8d5..94361bd3fbd4a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -32,14 +32,79 @@ AvoidanceModuleManager::AvoidanceModuleManager( void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) { + using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; auto p = parameters_; - std::string ns = "avoidance."; - updateParam(parameters, ns + "enable_safety_check", p->enable_safety_check); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + { + const std::string ns = "avoidance."; + updateParam(parameters, ns + "enable_safety_check", p->enable_safety_check); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + } + + const auto update_object_param = [&p, ¶meters]( + const auto & semantic, const std::string & ns) { + auto & config = p->object_parameters.at(semantic); + updateParam(parameters, ns + "enable", config.enable); + updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); + updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); + updateParam( + parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + }; + + { + const std::string ns = "avoidance.target_object."; + update_object_param(ObjectClassification::MOTORCYCLE, ns + "motorcycle."); + update_object_param(ObjectClassification::CAR, ns + "car."); + update_object_param(ObjectClassification::TRUCK, ns + "truck."); + update_object_param(ObjectClassification::TRAILER, ns + "trailer."); + update_object_param(ObjectClassification::BUS, ns + "bus."); + update_object_param(ObjectClassification::PEDESTRIAN, ns + "pedestrian."); + update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); + update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); + } + + { + const std::string ns = "avoidance.avoidance.lateral."; + updateParam( + parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold); + updateParam(parameters, ns + "lateral_collision_margin", p->lateral_collision_margin); + updateParam( + parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin); + } + + { + const std::string ns = "avoidance.avoidance.longitudinal."; + updateParam(parameters, ns + "prepare_time", p->prepare_time); + } + + { + const std::string ns = "avoidance.stop."; + updateParam(parameters, ns + "max_distance", p->stop_max_distance); + updateParam(parameters, ns + "min_distance", p->stop_min_distance); + } + + { + const std::string ns = "avoidance.constrains.lateral."; + updateParam(parameters, ns + "nominal_lateral_jerk", p->nominal_lateral_jerk); + updateParam(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk); + } + + { + const std::string ns = "avoidance.shift_line_pipeline."; + updateParam( + parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); + updateParam( + parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); + } std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { m->updateModuleParams(p); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp deleted file mode 100644 index 3e756ddd99149..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" - -#include - -#include -#include -#include - -namespace behavior_path_planner -{ - -AvoidanceByLCModuleManager::AvoidanceByLCModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {"left", "right"}), parameters_{parameters} -{ -} - -void AvoidanceByLCModuleManager::updateModuleParams( - [[maybe_unused]] const std::vector & parameters) -{ - using tier4_autoware_utils::updateParam; - - [[maybe_unused]] auto p = parameters_; - - [[maybe_unused]] const std::string ns = name_ + "."; - - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); - }); -} - -} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp deleted file mode 100644 index 25b7b2e7bc1ed..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp +++ /dev/null @@ -1,1007 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/avoidance_by_lc/module.hpp" - -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/avoidance/util.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include -#include -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ - -using autoware_auto_perception_msgs::msg::ObjectClassification; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::toHexString; - -AvoidanceByLCModule::AvoidanceByLCModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} -{ -} - -void AvoidanceByLCModule::processOnEntry() -{ -#ifndef USE_OLD_ARCHITECTURE - waitApproval(); -#endif - current_lane_change_state_ = LaneChangeStates::Normal; - updateLaneChangeStatus(); -} - -void AvoidanceByLCModule::processOnExit() -{ - resetParameters(); -} - -bool AvoidanceByLCModule::isExecutionRequested() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - if (!found_valid_path) { - return false; - } - - const auto object_num = avoidance_data_.target_objects.size(); - if (parameters_->execute_object_num > object_num) { - return false; - } - - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - if (parameters_->execute_object_longitudinal_margin > to_front_object_distance) { - return false; - } - - const auto to_lane_change_end_distance = calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - const auto lane_change_finish_before_object = - to_front_object_distance > to_lane_change_end_distance; - if ( - !lane_change_finish_before_object && - parameters_->execute_only_when_lane_change_finish_before_object) { - return false; - } - - return true; -} - -bool AvoidanceByLCModule::isExecutionReady() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_safe_path; -} - -void AvoidanceByLCModule::updateData() -{ - debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); - - updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_->avoidance); - compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); - - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); -} - -AvoidancePlanningData AvoidanceByLCModule::calcAvoidancePlanningData(DebugData & debug) const -{ - AvoidancePlanningData data; - - // reference pose - const auto reference_pose = getEgoPose(); - data.reference_pose = reference_pose; - - data.reference_path = utils::resamplePathWithSpline( - *getPreviousModuleOutput().path, parameters_->avoidance->resample_interval_for_planning); - - const size_t nearest_segment_index = - findNearestSegmentIndex(data.reference_path.points, data.reference_pose.position); - data.ego_closest_path_index = - std::min(nearest_segment_index + 1, data.reference_path.points.size() - 1); - - // arclength from ego pose (used in many functions) - data.arclength_from_ego = utils::calcPathArcLengthArray( - data.reference_path, 0, data.reference_path.points.size(), - calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); - - // lanelet info -#ifdef USE_OLD_ARCHITECTURE - data.current_lanelets = utils::getCurrentLanes(planner_data_); -#else - data.current_lanelets = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - // target objects for avoidance - fillAvoidanceTargetObjects(data, debug); - - return data; -} - -void AvoidanceByLCModule::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const -{ - const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - data.current_lanelets, parameters_->avoidance->detection_area_left_expand_dist, - parameters_->avoidance->detection_area_right_expand_dist * (-1.0)); - - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); - - for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; - other_object.reason = "OutOfTargetArea"; - data.other_objects.push_back(other_object); - } - - ObjectDataArray objects; - for (const auto & object : object_within_target_lane.objects) { - objects.push_back(createObjectData(data, object)); - } - - filterTargetObjects(objects, data, debug, planner_data_, parameters_->avoidance); -} - -ObjectData AvoidanceByLCModule::createObjectData( - const AvoidancePlanningData & data, const PredictedObject & object) const -{ - using boost::geometry::return_centroid; - - const auto & path_points = data.reference_path.points; - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - - ObjectData object_data{}; - - object_data.object = object; - - // Calc envelop polygon. - fillObjectEnvelopePolygon( - object_data, registered_objects_, object_closest_pose, parameters_->avoidance); - - // calc object centroid. - object_data.centroid = return_centroid(object_data.envelope_poly); - - // calc longitudinal distance from ego to closest target object footprint point. - fillLongitudinalAndLengthByClosestEnvelopeFootprint( - data.reference_path, getEgoPosition(), object_data); - - // Calc moving time. - fillObjectMovingTime(object_data, stopped_objects_, parameters_->avoidance); - - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto safety_margin = - 0.5 * vehicle_width + parameters_->avoidance->lateral_passable_safety_buffer; - object_data.avoid_required = - (isOnRight(object_data) && std::abs(object_data.overhang_dist) < safety_margin) || - (!isOnRight(object_data) && object_data.overhang_dist < safety_margin); - - return object_data; -} - -ModuleStatus AvoidanceByLCModule::updateState() -{ - RCLCPP_DEBUG(getLogger(), "AVOIDANCE_BY_LC updateState"); - if (!isValidPath()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - - if (isWaitingApproval()) { - const auto object_num = avoidance_data_.target_objects.size(); - if (parameters_->execute_object_num > object_num) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - if (parameters_->execute_object_longitudinal_margin > to_front_object_distance) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - const auto to_lane_change_end_distance = calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.shift_line.end.position); - const auto lane_change_finish_before_object = - to_front_object_distance > to_lane_change_end_distance; - if ( - !lane_change_finish_before_object && - parameters_->execute_only_when_lane_change_finish_before_object) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - } - - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (isAbortState() && !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentVelocityLow()) || !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - if (hasFinishedLaneChange()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - current_state_ = ModuleStatus::RUNNING; - return current_state_; -} - -BehaviorModuleOutput AvoidanceByLCModule::plan() -{ - resetPathCandidate(); - resetPathReference(); - is_activated_ = isActivated(); - - PathWithLaneId path = status_.lane_change_path.path; - if (!isValidPath(path)) { - status_.is_valid_path = false; - return BehaviorModuleOutput{}; - } else { - status_.is_valid_path = true; - } - - if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentVelocityLow())) { - const auto stop_point = utils::insertStopPoint(0.1, path); - } - - if (isAbortState()) { - resetPathIfAbort(); - if (is_activated_) { - path = abort_path_->path; - } - } - - generateExtendedDrivableArea(path); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); -#ifdef USE_OLD_ARCHITECTURE - path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = path; -#else - const auto reference_path = - utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_); - output.reference_path = std::make_shared(reference_path); - path_reference_ = std::make_shared(reference_path); - prev_approved_path_ = *getPreviousModuleOutput().path; -#endif - updateOutputTurnSignal(output); - - updateSteeringFactorPtr(output); - clearWaitingApproval(); - - return output; -} - -void AvoidanceByLCModule::resetPathIfAbort() -{ - if (!is_abort_approval_requested_) { -#ifdef USE_OLD_ARCHITECTURE - const auto lateral_shift = utils::lane_change::getLateralShift(*abort_path_); - if (lateral_shift > 0.0) { - removePreviousRTCStatusRight(); - uuid_map_.at("right") = generateUUID(); - } else if (lateral_shift < 0.0) { - removePreviousRTCStatusLeft(); - uuid_map_.at("left") = generateUUID(); - } -#else - removeRTCStatus(); -#endif - RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - is_abort_approval_requested_ = true; - is_abort_path_approved_ = false; - return; - } - - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); - is_abort_path_approved_ = true; - clearWaitingApproval(); - } else { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); - is_abort_path_approved_ = false; - waitApproval(); - } -} - -CandidateOutput AvoidanceByLCModule::planCandidate() const -{ - CandidateOutput output; - - LaneChangePath selected_path; - // Get lane change lanes -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - if (lane_change_lanes.empty()) { - return output; - } - -#ifdef USE_OLD_ARCHITECTURE - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); -#else - selected_path = status_.lane_change_path; -#endif - - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - - if (isAbortState()) { - selected_path = *abort_path_; - } - - if (selected_path.path.points.empty()) { - return output; - } - - output.path_candidate = selected_path.path; - output.lateral_shift = utils::lane_change::getLateralShift(selected_path); - output.start_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - - updateSteeringFactorPtr(output, selected_path); - return output; -} - -BehaviorModuleOutput AvoidanceByLCModule::planWaitingApproval() -{ -#ifdef USE_OLD_ARCHITECTURE - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (is_within_current_lane) { - prev_approved_path_ = getReferencePath(); - } -#else - prev_approved_path_ = *getPreviousModuleOutput().path; -#endif - BehaviorModuleOutput out; - out.path = std::make_shared(prev_approved_path_); - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - - if (!avoidance_data_.target_objects.empty()) { - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - const auto lane_change_buffer = planner_data_->parameters.minimum_lane_changing_length; - - boost::optional p_insert{}; - insertDecelPoint( - getEgoPosition(), to_front_object_distance - lane_change_buffer, 0.0, *out.path, p_insert); - } - -#ifndef USE_OLD_ARCHITECTURE - updateLaneChangeStatus(); -#endif - - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; - updateRTCStatus(candidate); - waitApproval(); - is_abort_path_approved_ = false; - - return out; -} - -void AvoidanceByLCModule::updateLaneChangeStatus() -{ -#ifdef USE_OLD_ARCHITECTURE - status_.current_lanes = utils::getCurrentLanes(planner_data_); -#else - status_.current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - status_.lane_change_lanes = getLaneChangeLanes(status_.current_lanes, lane_change_lane_length_); - - // Find lane change path - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(status_.lane_change_lanes, check_distance_, selected_path); - - // Update status - status_.is_safe = found_safe_path; - status_.lane_change_path = selected_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.lane_change_lanes); - - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, getEgoPose()); - status_.start_distance = arclength_start.length; - status_.lane_change_path.path.header = getRouteHeader(); -} - -PathWithLaneId AvoidanceByLCModule::getReferencePath() const -{ - PathWithLaneId reference_path; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto & common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = getRouteHeader(); - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - if (current_lanes.empty()) { - return reference_path; - } - - if (reference_path.points.empty()) { - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); - } - - const int num_lane_change = - std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, 0.0); - - const double lane_change_buffer = - utils::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); - - reference_path = utils::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_->lane_change->prepare_duration, - lane_change_buffer); - - const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->lane_change->drivable_area_left_bound_offset, - parameters_->lane_change->drivable_area_right_bound_offset, - parameters_->lane_change->drivable_area_types_to_skip); - utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); - - return reference_path; -} - -lanelet::ConstLanelets AvoidanceByLCModule::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const -{ - lanelet::ConstLanelets lane_change_lanes; - const auto & route_handler = planner_data_->route_handler; - const auto minimum_lane_changing_length = planner_data_->parameters.minimum_lane_changing_length; - const auto prepare_duration = parameters_->lane_change->prepare_duration; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - - if (current_lanes.empty()) { - return lane_change_lanes; - } - - const auto object_num = avoidance_data_.target_objects.size(); - if (object_num < parameters_->execute_object_num) { - return lane_change_lanes; - } - - const auto o_front = avoidance_data_.target_objects.front(); - - // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); - const double lane_change_prepare_length = - std::max(current_twist.linear.x * prepare_duration, minimum_lane_changing_length); - lanelet::ConstLanelets current_check_lanes = - route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); - lanelet::ConstLanelet lane_change_lane; - - if (isOnRight(o_front)) { - for (const auto & lanelet : current_check_lanes) { - const auto & left_lane = route_handler->getRoutingGraphPtr()->left(lanelet); - if (left_lane) { - lane_change_lanes = route_handler->getLaneletSequence( - left_lane.get(), current_pose, lane_change_lane_length, lane_change_lane_length); - break; - } - } - } else { - for (const auto & lanelet : current_check_lanes) { - const auto & right_lane = route_handler->getRoutingGraphPtr()->right(lanelet); - if (right_lane) { - lane_change_lanes = route_handler->getLaneletSequence( - right_lane.get(), current_pose, lane_change_lane_length, lane_change_lane_length); - break; - } - } - } - - return lane_change_lanes; -} - -std::pair AvoidanceByLCModule::getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & common_parameters = planner_data_->parameters; - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - if (lane_change_lanes.empty()) { - return std::make_pair(false, false); - } - - if (avoidance_data_.target_objects.empty()) { - return std::make_pair(false, false); - } - - // find candidate paths - LaneChangePaths valid_paths; -#ifdef USE_OLD_ARCHITECTURE - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - planner_data_->dynamic_object, common_parameters, *parameters_->lane_change, check_distance, - &valid_paths, &object_debug_); -#else - const auto o_front = avoidance_data_.target_objects.front(); - const auto direction = isOnRight(o_front) ? Direction::LEFT : Direction::RIGHT; - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *getPreviousModuleOutput().path, *route_handler, current_lanes, lane_change_lanes, current_pose, - current_twist, planner_data_->dynamic_object, common_parameters, *parameters_->lane_change, - check_distance, direction, &valid_paths, &object_debug_); -#endif - debug_valid_path_ = valid_paths; - - if (parameters_->lane_change->publish_debug_marker) { - setObjectDebugVisualization(); - } else { - debug_marker_.markers.clear(); - } - - if (valid_paths.empty()) { - return {false, false}; - } - - if (found_safe_path) { - safe_path = valid_paths.back(); - } else { - // force candidate - safe_path = valid_paths.front(); - } - - return {true, found_safe_path}; -} - -bool AvoidanceByLCModule::isSafe() const -{ - return status_.is_safe; -} - -bool AvoidanceByLCModule::isValidPath() const -{ - return status_.is_valid_path; -} - -bool AvoidanceByLCModule::isValidPath(const PathWithLaneId & path) const -{ - const auto & route_handler = planner_data_->route_handler; - - // check lane departure - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.lane_change_lanes)); - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->lane_change->drivable_area_left_bound_offset, - parameters_->lane_change->drivable_area_right_bound_offset); - const auto lanelets = utils::transformToLanelets(expanded_lanes); - - // check path points are in any lanelets - for (const auto & point : path.points) { - bool is_in_lanelet = false; - for (const auto & lanelet : lanelets) { - if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { - is_in_lanelet = true; - break; - } - } - if (!is_in_lanelet) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; - } - } - - // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); - return false; - } - - return true; -} - -bool AvoidanceByLCModule::isNearEndOfLane() const -{ - const auto & current_pose = getEgoPose(); - const double threshold = utils::calcTotalLaneChangeLength(planner_data_->parameters); - - return std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < - threshold; -} - -bool AvoidanceByLCModule::isCurrentVelocityLow() const -{ - constexpr double threshold_ms = 10.0 * 1000 / 3600; - return utils::l2Norm(getEgoTwist().linear) < threshold_ms; -} - -bool AvoidanceByLCModule::isAbortConditionSatisfied() -{ - is_abort_condition_satisfied_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - - if (!parameters_->lane_change->enable_cancel_lane_change) { - return false; - } - - if (!is_activated_) { - return false; - } - - Pose ego_pose_before_collision; - const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - - if (!is_path_safe) { - const auto & common_parameters = planner_data_->parameters; - const bool is_within_original_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), common_parameters); - - if (is_within_original_lane) { - current_lane_change_state_ = LaneChangeStates::Cancel; - return true; - } - - // check abort enable flag - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); - - if (!parameters_->lane_change->enable_abort_lane_change) { - current_lane_change_state_ = LaneChangeStates::Stop; - return false; - } - - const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, - *parameters_->lane_change); - - if (!found_abort_path && !is_abort_path_approved_) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - - current_lane_change_state_ = LaneChangeStates::Abort; - - if (!is_abort_path_approved_) { - abort_path_ = std::make_shared(*found_abort_path); - } - - return true; - } - - return false; -} - -bool AvoidanceByLCModule::isAbortState() const -{ - if (!parameters_->lane_change->enable_abort_lane_change) { - return false; - } - - if (current_lane_change_state_ != LaneChangeStates::Abort) { - return false; - } - - if (!abort_path_) { - return false; - } - - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); - return true; -} - -bool AvoidanceByLCModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - - const auto & current_pose = getEgoPose(); - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); - - return std::abs(arclength_current.distance) > AVOIDING_SHIFT_THR; -} - -bool AvoidanceByLCModule::hasFinishedLaneChange() const -{ - const auto & current_pose = getEgoPose(); - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); - const double travel_distance = arclength_current.length - status_.start_distance; - const double finish_distance = status_.lane_change_path.length.sum() + - parameters_->lane_change->lane_change_finish_judge_buffer; - return travel_distance > finish_distance; -} - -void AvoidanceByLCModule::setObjectDebugVisualization() const -{ - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; - - debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showObjectInfo(object_debug_, "object_debug_info")); - add(showLerpedPose(object_debug_, "lerp_pose_before_true")); - add(showPolygonPose(object_debug_, "expected_pose")); - add(showPolygon(object_debug_, "lerped_polygon")); - add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths")); -} - -std::shared_ptr AvoidanceByLCModule::get_debug_msg_array() const -{ - LaneChangeDebugMsgArray debug_msg_array; - debug_msg_array.lane_change_info.reserve(object_debug_.size()); - for (const auto & [uuid, debug_data] : object_debug_) { - LaneChangeDebugMsg debug_msg; - debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; - debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = utils::l2Norm(debug_data.object_twist.linear); - debug_msg_array.lane_change_info.push_back(debug_msg); - } - lane_change_debug_msg_array_ = debug_msg_array; - - lane_change_debug_msg_array_.header.stamp = clock_->now(); - return std::make_shared(lane_change_debug_msg_array_); -} - -void AvoidanceByLCModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) -{ - const auto turn_signal_info = output.turn_signal_info; - const auto current_pose = getEgoPose(); - const double start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); - - const uint16_t steering_factor_direction = - std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - waitApprovalLeft(start_distance, finish_distance); - return SteeringFactor::LEFT; - } - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - waitApprovalRight(start_distance, finish_distance); - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); - - // TODO(tkhmy) add handle status TRYING - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.lane_change_path.shift_line.start, status_.lane_change_path.shift_line.end}, - {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, - SteeringFactor::TURNING, ""); -} - -void AvoidanceByLCModule::updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const -{ - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - - steering_factor_interface_ptr_->updateSteeringFactor( - {selected_path.shift_line.start, selected_path.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); -} - -std_msgs::msg::Header AvoidanceByLCModule::getRouteHeader() const -{ - return planner_data_->route_handler->getRouteHeader(); -} - -void AvoidanceByLCModule::generateExtendedDrivableArea(PathWithLaneId & path) -{ - const auto & common_parameters = planner_data_->parameters; - const auto & route_handler = planner_data_->route_handler; - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, status_.current_lanes, status_.lane_change_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->lane_change->drivable_area_left_bound_offset, - parameters_->lane_change->drivable_area_right_bound_offset); - utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); -} - -bool AvoidanceByLCModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const -{ - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = parameters_->lane_change; - const auto & route_handler = planner_data_->route_handler; - const auto & path = status_.lane_change_path; - - // get lanes used for detection - const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - *route_handler, path.target_lanelets.front(), current_pose, check_distance_); - - std::unordered_map debug_data; - const auto lateral_buffer = - utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); - const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, - *lane_change_parameters, lateral_buffer); - - return utils::lane_change::isLaneChangePathSafe( - path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_->lane_change, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, - status_.lane_change_path.acceleration); -} - -void AvoidanceByLCModule::updateOutputTurnSignal(BehaviorModuleOutput & output) -{ - const auto turn_signal_info = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - output.turn_signal_info.turn_signal.command = turn_signal_info.first.command; - - utils::lane_change::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); -} - -void AvoidanceByLCModule::resetParameters() -{ - is_abort_path_approved_ = false; - is_abort_approval_requested_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - abort_path_ = nullptr; - - object_debug_.clear(); - debug_marker_.markers.clear(); - resetPathCandidate(); - resetPathReference(); -} - -void AvoidanceByLCModule::acceptVisitor(const std::shared_ptr & visitor) const -{ - if (visitor) { - visitor->visitAvoidanceByLCModule(this); - } -} - -void SceneModuleVisitor::visitAvoidanceByLCModule( - [[maybe_unused]] const AvoidanceByLCModule * module) const -{ - // lane_change_visitor_ = module->get_debug_msg_array(); -} -} // namespace behavior_path_planner 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 new file mode 100644 index 0000000000000..47c29be29c2a3 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -0,0 +1,561 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +namespace +{ +bool isCentroidWithinLanelets( + const geometry_msgs::msg::Point & obj_pos, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + lanelet::BasicPoint2d object_centroid(obj_pos.x, obj_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +std::vector getObjectsInLanes( + const std::vector & objects, + const lanelet::ConstLanelets & target_lanes) +{ + std::vector target_objects; + for (const auto & object : objects) { + if (isCentroidWithinLanelets(object.pose.position, target_lanes)) { + target_objects.push_back(object); + } + } + + return target_objects; +} + +geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) +{ + geometry_msgs::msg::Point geom_obj_point; + geom_obj_point.x = point.x(); + geom_obj_point.y = point.y(); + return geom_obj_point; +} + +double calcObstacleProjectedVelocity( + const std::vector & path_points, const PredictedObject & object) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + + const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); + + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); + + return obj_vel * std::cos(obj_yaw - path_yaw); +} + +std::pair getMinMaxValues(const std::vector & vec) +{ + const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); + + const size_t max_idx = std::distance(vec.begin(), std::max_element(vec.begin(), vec.end())); + + return std::make_pair(vec.at(min_idx), vec.at(max_idx)); +} + +void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) +{ + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", + marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, + tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + marker.pose = obj_pose; + + marker_array.markers.push_back(marker); +} + +void appendExtractedPolygonMarker( + MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly) +{ + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), + visualization_msgs::msg::Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + + // NOTE: obj_poly.outer() has already duplicated points to close the polygon. + for (size_t i = 0; i < obj_poly.outer().size(); ++i) { + const auto & bound_point = obj_poly.outer().at(i); + + geometry_msgs::msg::Point bound_geom_point; + bound_geom_point.x = bound_point.x(); + bound_geom_point.y = bound_point.y(); + marker.points.push_back(bound_geom_point); + } + + marker_array.markers.push_back(marker); +} +} // namespace + +#ifdef USE_OLD_ARCHITECTURE +DynamicAvoidanceModule::DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters) +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, + parameters_{std::move(parameters)} +#else +DynamicAvoidanceModule::DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters, + const std::unordered_map> & rtc_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} +#endif +{ +} + +bool DynamicAvoidanceModule::isExecutionRequested() const +{ + RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); + + const auto prev_module_path = getPreviousModuleOutput().path; + if (!prev_module_path || prev_module_path->points.size() < 2) { + return false; + } + + // check if the ego is driving forward + const auto is_driving_forward = motion_utils::isDrivingForward(prev_module_path->points); + if (!is_driving_forward || !(*is_driving_forward)) { + return false; + } + + // check if the planner is already running + if (current_state_ == ModuleStatus::RUNNING) { + return true; + } + + // check if there is target objects to avoid + return !target_objects_.empty(); +} + +bool DynamicAvoidanceModule::isExecutionReady() const +{ + RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionReady."); + return true; +} + +void DynamicAvoidanceModule::updateData() +{ + target_objects_ = calcTargetObjects(); +} + +ModuleStatus DynamicAvoidanceModule::updateState() +{ + const bool has_avoidance_target = !target_objects_.empty(); + + if (!has_avoidance_target) { + return ModuleStatus::SUCCESS; + } + + return ModuleStatus::RUNNING; +} + +BehaviorModuleOutput DynamicAvoidanceModule::plan() +{ + info_marker_.markers.clear(); + debug_marker_.markers.clear(); + + // 1. get reference path from previous module + const auto prev_module_path = getPreviousModuleOutput().path; + + // 2. get drivable lanes from previous module + const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; + + // 3. create obstacles to avoid (= extract from the drivable area) + std::vector obstacles_for_drivable_area; + for (const auto & object : target_objects_) { + const auto obstacle_poly = calcDynamicObstaclePolygon(object); + if (obstacle_poly) { + obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value(), object.is_left}); + + appendObjectMarker(info_marker_, object.pose); + appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); + } + } + + BehaviorModuleOutput output; + output.path = prev_module_path; + output.reference_path = getPreviousModuleOutput().reference_path; + // for new architecture + output.drivable_area_info.drivable_lanes = drivable_lanes; + output.drivable_area_info.obstacles = obstacles_for_drivable_area; + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + + return output; +} + +CandidateOutput DynamicAvoidanceModule::planCandidate() const +{ + auto candidate_path = utils::generateCenterLinePath(planner_data_); + return CandidateOutput(*candidate_path); +} + +BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval() +{ + BehaviorModuleOutput out = plan(); + return out; +} + +bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + + if (label == ObjectClassification::CAR && parameters_->avoid_car) { + return true; + } + if (label == ObjectClassification::TRUCK && parameters_->avoid_truck) { + return true; + } + if (label == ObjectClassification::BUS && parameters_->avoid_bus) { + return true; + } + if (label == ObjectClassification::TRAILER && parameters_->avoid_trailer) { + return true; + } + if (label == ObjectClassification::UNKNOWN && parameters_->avoid_unknown) { + return true; + } + if (label == ObjectClassification::BICYCLE && parameters_->avoid_bicycle) { + return true; + } + if (label == ObjectClassification::MOTORCYCLE && parameters_->avoid_motorcycle) { + return true; + } + if (label == ObjectClassification::PEDESTRIAN && parameters_->avoid_pedestrian) { + return true; + } + return false; +} + +std::vector +DynamicAvoidanceModule::calcTargetObjects() const +{ + const auto prev_module_path = getPreviousModuleOutput().path; + const auto & predicted_objects = planner_data_->dynamic_object->objects; + + // 1. convert predicted objects to dynamic avoidance objects + std::vector input_objects; + for (const auto & predicted_object : predicted_objects) { + // check label + const bool is_label_target_obstacle = + isLabelTargetObstacle(predicted_object.classification.front().label); + if (!is_label_target_obstacle) { + continue; + } + + const double path_projected_vel = + calcObstacleProjectedVelocity(prev_module_path->points, predicted_object); + // check if velocity is high enough + if (std::abs(path_projected_vel) < parameters_->min_obstacle_vel) { + continue; + } + + input_objects.push_back(DynamicAvoidanceObject(predicted_object, path_projected_vel)); + } + + // 2. calculate target lanes to filter obstacles + const auto [right_lanes, left_lanes] = getAdjacentLanes(100.0, 50.0); + + // 3. filter obstacles for dynamic avoidance + const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); + const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); + + // 4. check if object will cut into the ego lane. + // NOTE: The oncoming object will be ignored. + constexpr double epsilon_path_lat_diff = 0.3; + std::vector output_objects; + for (const bool is_left : {true, false}) { + for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { + const auto reliable_predicted_path = std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { + return a.confidence < b.confidence; + }); + + // Ignore object that will cut into the ego lane + const bool will_object_cut_in = [&]() { + if (object.path_projected_vel < 0) { + // Ignore oncoming object + return false; + } + + for (const auto & predicted_path_point : reliable_predicted_path->path) { + const double paths_lat_diff = motion_utils::calcLateralOffset( + prev_module_path->points, predicted_path_point.position); + if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { + return true; + } + } + return false; + }(); + if (will_object_cut_in) { + continue; + } + + auto target_object = object; + target_object.is_left = is_left; + output_objects.push_back(target_object); + } + } + + return output_objects; +} + +std::pair DynamicAvoidanceModule::getAdjacentLanes( + const double forward_distance, const double backward_distance) const +{ + const auto & rh = planner_data_->route_handler; + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); + + lanelet::ConstLanelets right_lanes; + lanelet::ConstLanelets left_lanes; + for (const auto & lane : ego_succeeding_lanes) { + // left lane + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (opt_left_lane) { + left_lanes.push_back(opt_left_lane.get()); + } + + // right lane + const auto opt_right_lane = rh->getRightLanelet(lane); + if (opt_right_lane) { + right_lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (!right_opposite_lanes.empty()) { + right_lanes.push_back(right_opposite_lanes.front()); + } + } + + return std::make_pair(right_lanes, left_lanes); +} + +std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + const auto ego_pose = getEgoPose(); + const auto & rh = planner_data_->route_handler; + + // get path with backward margin + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), + "failed to find closest lanelet within route!!!"); + return std::nullopt; + } + + auto path_with_backward_margin = [&]() { + constexpr double forward_length = 100.0; + const double backward_length = 50.0; + const auto current_lanes = + rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); + return utils::getCenterLinePath( + *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); + }(); + + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, object.pose.position); + const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + + // calculate min/max lateral offset from object to path + const auto [min_obj_lat_abs_offset, max_obj_lat_abs_offset] = [&]() { + std::vector obj_lat_abs_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, geom_obj_point); + const double obj_point_lat_offset = motion_utils::calcLateralOffset( + path_with_backward_margin.points, geom_obj_point, obj_point_seg_idx); + obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); + } + return getMinMaxValues(obj_lat_abs_offset_vec); + }(); + const double min_obj_lat_offset = min_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0); + const double max_obj_lat_offset = max_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0); + + // calculate min/max longitudinal offset from object to path + const auto obj_lon_offset = [&]() -> std::optional> { + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( + path_with_backward_margin.points, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); + } + + const auto [raw_min_obj_lon_offset, raw_max_obj_lon_offset] = + getMinMaxValues(obj_lon_offset_vec); + + // calculate time to collision and apply it to drivable area extraction + const double relative_velocity = getEgoSpeed() - object.path_projected_vel; + const double time_to_collision = [&]() { + const auto prev_module_path = getPreviousModuleOutput().path; + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position); + const double signed_lon_length = motion_utils::calcSignedArcLength( + prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx); + if (relative_velocity == 0.0) { + return std::numeric_limits::max(); + } + return signed_lon_length / relative_velocity; + }(); + + if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + return std::nullopt; + } + + if (0 <= object.path_projected_vel) { + const double limited_time_to_collision = + std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision); + return std::make_pair( + raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision, + raw_max_obj_lon_offset + object.path_projected_vel * limited_time_to_collision); + } + + const double limited_time_to_collision = + std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision); + return std::make_pair( + raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision, + raw_max_obj_lon_offset); + }(); + + if (!obj_lon_offset) { + return std::nullopt; + } + const double min_obj_lon_offset = obj_lon_offset->first; + const double max_obj_lon_offset = obj_lon_offset->second; + + // calculate bound start and end index + const bool is_object_overtaking = (0.0 <= object.path_projected_vel); + const double start_length_to_avoid = + std::abs(object.path_projected_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(object.path_projected_vel) * (is_object_overtaking + ? parameters_->end_duration_to_avoid_overtaking_object + : parameters_->end_duration_to_avoid_oncoming_object); + const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( + obj_seg_idx, min_obj_lon_offset - start_length_to_avoid, path_with_backward_margin.points); + const size_t updated_obj_seg_idx = + (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 + : obj_seg_idx; + const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( + updated_obj_seg_idx, max_obj_lon_offset + end_length_to_avoid, + path_with_backward_margin.points); + + if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { + // NOTE: The obstacle is longitudinally out of the ego's trajectory. + return std::nullopt; + } + const size_t lon_bound_start_idx = + lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); + const size_t lon_bound_end_idx = + lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() + : static_cast(path_with_backward_margin.points.size() - 1); + + // calculate bound min and max lateral offset + const double min_bound_lat_offset = [&]() { + const double raw_min_bound_lat_offset = + min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); + const double min_bound_lat_abs_offset_limit = + planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; + if (object.is_left) { + if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) { + return min_bound_lat_abs_offset_limit; + } + } else { + if (-min_bound_lat_abs_offset_limit < raw_min_bound_lat_offset) { + return -min_bound_lat_abs_offset_limit; + } + } + return raw_min_bound_lat_offset; + }(); + const double max_bound_lat_offset = + max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); + + // create inner/outer bound points + std::vector obj_inner_bound_points; + std::vector obj_outer_bound_points; + for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { + obj_inner_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + path_with_backward_margin.points.at(i).point.pose, 0.0, min_bound_lat_offset, 0.0) + .position); + obj_outer_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + path_with_backward_margin.points.at(i).point.pose, 0.0, max_bound_lat_offset, 0.0) + .position); + } + + // create obj_polygon from inner/outer bound points + tier4_autoware_utils::Polygon2d obj_poly; + for (const auto & bound_point : obj_inner_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); + for (const auto & bound_point : obj_outer_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + + boost::geometry::correct(obj_poly); + return obj_poly; +} +} // namespace behavior_path_planner 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 new file mode 100644 index 0000000000000..d3212a7c0b1d4 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ + +DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + const std::shared_ptr & parameters) +: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} +{ +} + +void DynamicAvoidanceModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + auto & p = parameters_; + + { // target object + const std::string ns = "dynamic_avoidance.target_object."; + + updateParam(parameters, ns + "car", p->avoid_car); + updateParam(parameters, ns + "truck", p->avoid_truck); + updateParam(parameters, ns + "bus", p->avoid_bus); + updateParam(parameters, ns + "trailer", p->avoid_trailer); + updateParam(parameters, ns + "unknown", p->avoid_unknown); + updateParam(parameters, ns + "bicycle", p->avoid_bicycle); + updateParam(parameters, ns + "motorcycle", p->avoid_motorcycle); + updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); + + updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); + } + + { // drivable_area_generation + const std::string ns = "dynamic_avoidance.drivable_area_generation."; + + updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); + updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + + updateParam( + parameters, ns + "overtaking_object.max_time_to_collision", + p->max_time_to_collision_overtaking_object); + updateParam( + parameters, ns + "overtaking_object.start_duration_to_avoid", + p->start_duration_to_avoid_overtaking_object); + updateParam( + parameters, ns + "overtaking_object.end_duration_to_avoid", + p->end_duration_to_avoid_overtaking_object); + updateParam( + parameters, ns + "overtaking_object.duration_to_hold_avoidance", + p->duration_to_hold_avoidance_overtaking_object); + + updateParam( + parameters, ns + "oncoming_object.max_time_to_collision", + p->max_time_to_collision_oncoming_object); + updateParam( + parameters, ns + "oncoming_object.start_duration_to_avoid", + p->start_duration_to_avoid_oncoming_object); + updateParam( + parameters, ns + "oncoming_object.end_duration_to_avoid", + p->end_duration_to_avoid_oncoming_object); + } + + std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { + m->updateModuleParams(p); + }); +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp similarity index 55% rename from planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp rename to planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 8bcab291ae88c..96026cab532ec 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" +#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -48,21 +48,17 @@ using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner { #ifdef USE_OLD_ARCHITECTURE -PullOverModule::PullOverModule( +GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, - parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + const std::shared_ptr & parameters) +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters} { #else -PullOverModule::PullOverModule( +GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, + const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, - parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { #endif LaneDepartureChecker lane_departure_checker{}; @@ -70,22 +66,28 @@ PullOverModule::PullOverModule( occupancy_grid_map_ = std::make_shared(); + left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; + + // planner when goal modification is not allowed + fixed_goal_planner_ = std::make_unique(); + // set enabled planner if (parameters_->enable_shift_parking) { pull_over_planners_.push_back(std::make_shared( node, *parameters, lane_departure_checker, occupancy_grid_map_)); } - if (parameters_->enable_arc_forward_parking) { - constexpr bool is_forward = true; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, getGeometricPullOverParameters(), lane_departure_checker, - occupancy_grid_map_, is_forward)); - } - if (parameters_->enable_arc_backward_parking) { - constexpr bool is_forward = false; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, getGeometricPullOverParameters(), lane_departure_checker, - occupancy_grid_map_, is_forward)); + // currently only support geometric_parallel_parking for left side parking + if (left_side_parking_) { + if (parameters_->enable_arc_forward_parking) { + constexpr bool is_forward = true; + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); + } + if (parameters_->enable_arc_backward_parking) { + constexpr bool is_forward = false; + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); + } } if (pull_over_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -93,37 +95,35 @@ PullOverModule::PullOverModule( // set selected goal searcher // currently there is only one goal_searcher_type - const auto & vehicle_footprint = - createVehicleFootprint(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_footprint_ = createVehicleFootprint(vehicle_info); goal_searcher_ = - std::make_shared(*parameters, vehicle_footprint, occupancy_grid_map_); - - // for collision check with objects - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + std::make_shared(*parameters, vehicle_footprint_, occupancy_grid_map_); // timer callback for generating lane parking candidate paths const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); lane_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); lane_parking_timer_ = rclcpp::create_timer( - &node, clock_, lane_parking_period_ns, std::bind(&PullOverModule::onTimer, this), + &node, clock_, lane_parking_period_ns, std::bind(&GoalPlannerModule::onTimer, this), lane_parking_timer_cb_group_); // freespace parking if (parameters_->enable_freespace_parking) { - freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + freespace_planner_ = std::make_unique(node, *parameters, vehicle_info); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); freespace_parking_timer_ = rclcpp::create_timer( &node, clock_, freespace_parking_period_ns, - std::bind(&PullOverModule::onFreespaceParkingTimer, this), freespace_parking_timer_cb_group_); + std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this), + freespace_parking_timer_cb_group_); } resetStatus(); } -void PullOverModule::resetStatus() +void GoalPlannerModule::resetStatus() { PUllOverStatus initial_status{}; status_ = initial_status; @@ -134,7 +134,7 @@ void PullOverModule::resetStatus() } // This function is needed for waiting for planner_data_ -void PullOverModule::updateOccupancyGrid() +void GoalPlannerModule::updateOccupancyGrid() { if (!planner_data_->occupancy_grid) { RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "occupancy_grid is not ready"); @@ -144,7 +144,7 @@ void PullOverModule::updateOccupancyGrid() } // generate pull over candidate paths -void PullOverModule::onTimer() +void GoalPlannerModule::onTimer() { // already generated pull over candidate paths if (!pull_over_path_candidates_.empty()) { @@ -169,8 +169,8 @@ void PullOverModule::onTimer() const GoalCandidate & goal_candidate) { planner->setPlannerData(planner_data_); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - pull_over_path->goal_id = goal_candidate.id; - if (pull_over_path) { + if (pull_over_path && isCrossingPossible(*pull_over_path)) { + pull_over_path->goal_id = goal_candidate.id; path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -210,7 +210,7 @@ void PullOverModule::onTimer() mutex_.unlock(); } -void PullOverModule::onFreespaceParkingTimer() +void GoalPlannerModule::onFreespaceParkingTimer() { if (!planner_data_) { return; @@ -221,12 +221,13 @@ void PullOverModule::onFreespaceParkingTimer() const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; - if (isStuck() && is_new_costmap) { + constexpr double path_update_duration = 1.0; + if (isStuck() && is_new_costmap && needPathUpdate(path_update_duration)) { planFreespacePath(); } } -BehaviorModuleOutput PullOverModule::run() +BehaviorModuleOutput GoalPlannerModule::run() { current_state_ = ModuleStatus::RUNNING; updateOccupancyGrid(); @@ -240,137 +241,116 @@ BehaviorModuleOutput PullOverModule::run() return plan(); } -ParallelParkingParameters PullOverModule::getGeometricPullOverParameters() const +void GoalPlannerModule::initializeOccupancyGridMap() { - ParallelParkingParameters params{}; - - params.th_arrived_distance = parameters_->th_arrived_distance; - params.th_stopped_velocity = parameters_->th_stopped_velocity; - params.after_forward_parking_straight_distance = - parameters_->after_forward_parking_straight_distance; - params.after_backward_parking_straight_distance = - parameters_->after_backward_parking_straight_distance; - params.forward_parking_velocity = parameters_->forward_parking_velocity; - params.backward_parking_velocity = parameters_->backward_parking_velocity; - params.forward_parking_lane_departure_margin = parameters_->forward_parking_lane_departure_margin; - params.backward_parking_lane_departure_margin = - parameters_->backward_parking_lane_departure_margin; - params.arc_path_interval = parameters_->arc_path_interval; - params.maximum_deceleration = parameters_->maximum_deceleration; - params.max_steer_angle = parameters_->pull_over_max_steer_angle; - - return params; + OccupancyGridMapParam occupancy_grid_map_param{}; + const double margin = parameters_->occupancy_grid_collision_check_margin; + occupancy_grid_map_param.vehicle_shape.length = + planner_data_->parameters.vehicle_length + 2 * margin; + occupancy_grid_map_param.vehicle_shape.width = + planner_data_->parameters.vehicle_width + 2 * margin; + occupancy_grid_map_param.vehicle_shape.base2back = + planner_data_->parameters.base_link2rear + margin; + occupancy_grid_map_param.theta_size = parameters_->theta_size; + occupancy_grid_map_param.obstacle_threshold = parameters_->obstacle_threshold; + occupancy_grid_map_->setParam(occupancy_grid_map_param); } -void PullOverModule::processOnEntry() +void GoalPlannerModule::processOnEntry() { + const auto & route_handler = planner_data_->route_handler; + // Initialize occupancy grid map if (parameters_->use_occupancy_grid) { - OccupancyGridMapParam occupancy_grid_map_param{}; - const double margin = parameters_->occupancy_grid_collision_check_margin; - occupancy_grid_map_param.vehicle_shape.length = - planner_data_->parameters.vehicle_length + 2 * margin; - occupancy_grid_map_param.vehicle_shape.width = - planner_data_->parameters.vehicle_width + 2 * margin; - occupancy_grid_map_param.vehicle_shape.base2back = - planner_data_->parameters.base_link2rear + margin; - occupancy_grid_map_param.theta_size = parameters_->theta_size; - occupancy_grid_map_param.obstacle_threshold = parameters_->obstacle_threshold; - occupancy_grid_map_->setParam(occupancy_grid_map_param); + initializeOccupancyGridMap(); } // initialize when receiving new route - if ( - !last_received_time_ || - *last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { + if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status - parallel_parking_parameters_ = getGeometricPullOverParameters(); resetStatus(); - refined_goal_pose_ = calcRefinedGoal(planner_data_->route_handler->getGoalPose()); - if (parameters_->enable_goal_research) { + // calculate goal candidates + const Pose goal_pose = route_handler->getGoalPose(); + refined_goal_pose_ = calcRefinedGoal(goal_pose); + if (allow_goal_modification_) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = refined_goal_pose_; + goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; goal_candidates_.push_back(goal_candidate); } } - last_received_time_ = - std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); + last_received_time_ = std::make_unique(route_handler->getRouteHeader().stamp); } -void PullOverModule::processOnExit() +void GoalPlannerModule::processOnExit() { resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); } -bool PullOverModule::isExecutionRequested() const +bool GoalPlannerModule::isExecutionRequested() const { if (current_state_ == ModuleStatus::RUNNING) { return true; } - const auto & current_lanes = utils::getCurrentLanes(planner_data_); - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + const auto & route_handler = planner_data_->route_handler; - // check if goal_pose is far - const bool is_in_goal_route_section = - planner_data_->route_handler->isInGoalRouteSection(current_lanes.back()); - // current_lanes does not have the goal - if (!is_in_goal_route_section) { - return false; - } + // if current position is far from goal, do not execute pull over + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const Pose & goal_pose = route_handler->getGoalPose(); + lanelet::ConstLanelet current_lane{}; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - if (self_to_goal_arc_length > calcModuleRequestLength()) { + allow_goal_modification_ = + route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); + const double request_length = + allow_goal_modification_ ? calcModuleRequestLength() : parameters_->minimum_request_length; + const double backward_goal_search_length = + allow_goal_modification_ ? parameters_->backward_goal_search_length : 0.0; + if ( + self_to_goal_arc_length < -backward_goal_search_length || + self_to_goal_arc_length > request_length) { return false; } - // check if goal_pose is in shoulder lane - bool goal_is_in_shoulder_lane = false; - lanelet::Lanelet closest_shoulder_lanelet; - if (lanelet::utils::query::getClosestLanelet( - planner_data_->route_handler->getShoulderLanelets(), goal_pose, - &closest_shoulder_lanelet)) { - // check if goal pose is in shoulder lane - if (lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lanelet, 0.1)) { - const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose.position); - const auto goal_yaw = tf2::getYaw(goal_pose.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; - if (std::abs(angle_diff) < th_angle) { - goal_is_in_shoulder_lane = true; - } - } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!allow_goal_modification_) { + // check if goal_pose is in current_lanes. + return std::any_of( + current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { + return lanelet::utils::isInLanelet(goal_pose, current_lane); + }); } - if (!goal_is_in_shoulder_lane) return false; - // check if self pose is NOT in shoulder lane - bool self_is_in_shoulder_lane = false; - const auto self_pose = planner_data_->self_odometry->pose.pose; - if (lanelet::utils::query::getClosestLanelet( - planner_data_->route_handler->getShoulderLanelets(), self_pose, - &closest_shoulder_lanelet)) { - self_is_in_shoulder_lane = - lanelet::utils::isInLanelet(self_pose, closest_shoulder_lanelet, 0.1); + // if (A) or (B) is met execute pull over + // (A) target lane is `road` and same to the current lanes + // (B) target lane is `road_shoulder` and neighboring to the current lanes + const lanelet::ConstLanelets pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + lanelet::ConstLanelet target_lane{}; + lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + if (!isCrossingPossible(current_lane, target_lane)) { + return false; } - if (self_is_in_shoulder_lane) return false; return true; } -bool PullOverModule::isExecutionReady() const +bool GoalPlannerModule::isExecutionReady() const { return true; } -double PullOverModule::calcModuleRequestLength() const +double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance(0.0); if (!min_stop_distance) { @@ -383,19 +363,21 @@ double PullOverModule::calcModuleRequestLength() const return std::max(minimum_request_length, parameters_->minimum_request_length); } -Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const +Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { - lanelet::Lanelet closest_shoulder_lanelet; - lanelet::utils::query::getClosestLanelet( - planner_data_->route_handler->getShoulderLanelets(), goal_pose, &closest_shoulder_lanelet); + const lanelet::ConstLanelets pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + + lanelet::Lanelet closest_pull_over_lanelet{}; + lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); // calc closest center line pose - Pose center_pose; + Pose center_pose{}; { // find position const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); const auto segment = lanelet::utils::getClosestSegment( - lanelet::utils::to2D(lanelet_point), closest_shoulder_lanelet.centerline()); + lanelet::utils::to2D(lanelet_point), closest_pull_over_lanelet.centerline()); const auto p1 = segment.front().basicPoint(); const auto p2 = segment.back().basicPoint(); const auto direction_vector = (p2 - p1).normalized(); @@ -414,8 +396,8 @@ Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const center_pose.orientation = tf2::toMsg(tf_quat); } - const auto distance_from_left_bound = utils::getSignedDistanceFromShoulderLeftBoundary( - planner_data_->route_handler->getShoulderLanelets(), vehicle_footprint_, center_pose); + const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); if (!distance_from_left_bound) { RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); return goal_pose; @@ -428,14 +410,19 @@ Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus PullOverModule::updateState() +ModuleStatus GoalPlannerModule::updateState() { + // finish module only when the goal is fixed + if (!allow_goal_modification_ && hasFinishedGoalPlanner()) { + return ModuleStatus::SUCCESS; + } + // pull_out module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. return current_state_; } -bool PullOverModule::planFreespacePath() +bool GoalPlannerModule::planFreespacePath() { mutex_.lock(); goal_searcher_->update(goal_candidates_); @@ -457,21 +444,27 @@ bool PullOverModule::planFreespacePath() status_.current_path_idx = 0; status_.is_safe = true; modified_goal_pose_ = goal_candidate; + last_path_update_time_ = std::make_unique(clock_->now()); mutex_.unlock(); return true; } return false; } -bool PullOverModule::returnToLaneParking() +void GoalPlannerModule::returnToLaneParking() { + // return only before starting free space parking + if (!isStopped()) { + return; + } + if (!status_.lane_parking_pull_over_path) { - return false; + return; } const PathWithLaneId path = status_.lane_parking_pull_over_path->getFullPath(); if (checkCollision(path)) { - return false; + return; } const Point & current_point = planner_data_->self_odometry->pose.pose.position; @@ -479,156 +472,110 @@ bool PullOverModule::returnToLaneParking() const bool is_close_to_path = std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; if (!is_close_to_path) { - return false; + return; } mutex_.lock(); + status_.is_safe = true; status_.has_decided_path = false; status_.pull_over_path = status_.lane_parking_pull_over_path; status_.current_path_idx = 0; + last_path_update_time_ = std::make_unique(clock_->now()); mutex_.unlock(); - return true; + RCLCPP_INFO(getLogger(), "return to lane parking"); } -BehaviorModuleOutput PullOverModule::plan() +BehaviorModuleOutput GoalPlannerModule::plan() { - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - - resetPathCandidate(); - resetPathReference(); - - status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_); - status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); - status_.lanes = - utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); - - // Check if it needs to decide path - if (status_.is_safe) { - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, current_pose, std::numeric_limits::max(), M_PI_2); - if (ego_segment_idx) { - const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.pull_over_path->start_pose.position); - const auto dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); - if (dist_to_parking_start_pose < parameters_->decide_path_distance) { - status_.has_decided_path = true; - } - } + if (allow_goal_modification_) { + return planWithGoalModification(); + } else { + // for fixed goals, only minor path refinements are made, + // so other modules are always allowed to run. + setIsSimultaneousExecutableAsApprovedModule(true); + setIsSimultaneousExecutableAsCandidateModule(true); + fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); + return fixed_goal_planner_->plan(planner_data_); } +} - if (status_.has_decided_path) { - // Use decided path - if (!status_.has_requested_approval) { - // request approval again one the final path is decided - waitApproval(); - removeRTCStatus(); - steering_factor_interface_ptr_->clearSteeringFactors(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); - } - current_state_ = ModuleStatus::SUCCESS; // for breaking loop - status_.has_requested_approval = true; - } else if (isActivated() && isWaitingApproval()) { - // When it is approved again after path is decided - clearWaitingApproval(); - last_approved_time_ = std::make_unique(clock_->now()); - last_approved_pose_ = std::make_unique(current_pose); - - // decide velocity to guarantee turn signal lighting time - if (!status_.has_decided_velocity) { - auto & first_path = status_.pull_over_path->partial_paths.front(); - const auto vel = - static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); - for (auto & p : first_path.points) { - p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); - } - } - status_.has_decided_velocity = true; +void GoalPlannerModule::selectSafePullOverPath() +{ + // select safe lane pull over path from candidates + mutex_.lock(); + goal_searcher_->setPlannerData(planner_data_); + goal_searcher_->update(goal_candidates_); + const auto pull_over_path_candidates = pull_over_path_candidates_; + const auto goal_candidates = goal_candidates_; + mutex_.unlock(); + status_.is_safe = false; + for (const auto & pull_over_path : pull_over_path_candidates) { + // check if goal is safe + const auto goal_candidate_it = std::find_if( + goal_candidates.begin(), goal_candidates.end(), + [pull_over_path](const auto & goal_candidate) { + return goal_candidate.id == pull_over_path.goal_id; + }); + if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { + continue; } - if (isActivated() && last_approved_time_ != nullptr) { - // if using arc_path and finishing current_path, get next path - // enough time for turn signal - const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > - planner_data_->parameters.turn_signal_search_time; - - if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { - if (incrementPathIndex()) { - last_increment_time_ = std::make_unique(clock_->now()); - } - } + // check if path is valid and safe + if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { + continue; } - } else if (!pull_over_path_candidates_.empty()) { - // select safe path from pull over path candidates - goal_searcher_->setPlannerData(planner_data_); + + status_.is_safe = true; mutex_.lock(); - goal_searcher_->update(goal_candidates_); - const auto pull_over_path_candidates = pull_over_path_candidates_; - const auto goal_candidates = goal_candidates_; + status_.pull_over_path = std::make_shared(pull_over_path); + status_.current_path_idx = 0; + status_.lane_parking_pull_over_path = status_.pull_over_path; + modified_goal_pose_ = *goal_candidate_it; + last_path_update_time_ = std::make_unique(clock_->now()); mutex_.unlock(); - status_.is_safe = false; - for (const auto & pull_over_path : pull_over_path_candidates) { - // check if goal is safe - const auto goal_candidate_it = std::find_if( - goal_candidates.begin(), goal_candidates.end(), - [pull_over_path](const auto & goal_candidate) { - return goal_candidate.id == pull_over_path.goal_id; - }); - if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { - continue; - } - - // check if path is valid and safe - if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { - continue; - } - - status_.is_safe = true; - mutex_.lock(); - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.lane_parking_pull_over_path = status_.pull_over_path; - modified_goal_pose_ = *goal_candidate_it; - mutex_.unlock(); - break; - } + break; + } - // decelerate before the search area start - if (status_.is_safe) { - const auto search_start_pose = calcLongitudinalOffsetPose( - status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, - -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front); - auto & first_path = status_.pull_over_path->partial_paths.front(); + // decelerate before the search area start + if (status_.is_safe) { + const auto search_start_pose = calcLongitudinalOffsetPose( + status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, + -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front); + auto & first_path = status_.pull_over_path->partial_paths.front(); - if (search_start_pose) { - decelerateBeforeSearchStart(*search_start_pose, first_path); - } else { - // if already passed the search start pose, set pull_over_velocity to first_path. - for (auto & p : first_path.points) { - p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); - } + if (search_start_pose) { + decelerateBeforeSearchStart(*search_start_pose, first_path); + } else { + // if already passed the search start pose, set pull_over_velocity to first_path. + for (auto & p : first_path.points) { + p.point.longitudinal_velocity_mps = std::min( + p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); } } + } - // generate drivable area for each partial path - for (auto & path : status_.pull_over_path->partial_paths) { - const size_t ego_idx = planner_data_->findEgoIndex(path.points); - utils::clipPathLength(path, ego_idx, planner_data_->parameters); - const auto shorten_lanes = utils::cutOverlappedLanes(path, status_.lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - utils::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); - } + // generate drivable area for each partial path + for (auto & path : status_.pull_over_path->partial_paths) { + const size_t ego_idx = planner_data_->findEgoIndex(path.points); + utils::clipPathLength(path, ego_idx, planner_data_->parameters); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); + utils::generateDrivableArea( + path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); } +} + +void GoalPlannerModule::setLanes() +{ + status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_); + status_.pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + status_.lanes = + utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); +} - BehaviorModuleOutput output; +void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) +{ if (status_.is_safe) { // safe: use pull over path status_.stop_pose.reset(); @@ -640,68 +587,99 @@ BehaviorModuleOutput PullOverModule::plan() output.path = std::make_shared(current_path); output.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; } else { // not safe: use stop_path - if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { - // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = std::make_shared(generateStopPath()); - output.reference_path = getPreviousModuleOutput().reference_path; - status_.prev_stop_path = output.path; - // set stop path as pull over path - PullOverPath pull_over_path{}; - mutex_.lock(); - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.pull_over_path->partial_paths.push_back(*output.path); - mutex_.unlock(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); - } else { - // not_safe -> not_safe: use previous stop path - output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); - } + setStopPath(output); } - status_.prev_is_safe = status_.is_safe; - // return to lane parking if it is possible - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { - // return only before starting free space parking - if (!isStopped() || status_.during_freespace_parking) { - status_.during_freespace_parking = true; - } else if (returnToLaneParking()) { - RCLCPP_INFO(getLogger(), "return to lane parking"); - } - } + setDrivableAreaInfo(output); + + setModifiedGoal(output); + prev_goal_id_ = modified_goal_pose_->id; // set hazard and turn signal if (status_.has_decided_path) { - output.turn_signal_info = calcTurnSignalInfo(); + setTurnSignalInfo(output); } - const auto distance_to_path_change = calcDistanceToPathChange(); - updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); + // for the next loop setOutput(). + // this is used to determine whether to generate a new stop path or keep the current stop path. + status_.prev_is_safe = status_.is_safe; +} - setDebugData(); +void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) +{ + if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + // safe -> not_safe or no prev_stop_path: generate new stop_path + output.path = std::make_shared(generateStopPath()); + output.reference_path = getPreviousModuleOutput().reference_path; + status_.prev_stop_path = output.path; + // set stop path as pull over path + mutex_.lock(); + PullOverPath pull_over_path{}; + status_.pull_over_path = std::make_shared(pull_over_path); + status_.current_path_idx = 0; + status_.pull_over_path->partial_paths.push_back(*output.path); + last_path_update_time_ = std::make_unique(clock_->now()); + mutex_.unlock(); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); + } else { + // not_safe -> not_safe: use previous stop path + output.path = status_.prev_stop_path; + output.reference_path = getPreviousModuleOutput().reference_path; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); + } +} - // Publish the modified goal only when it is updated - if ( - status_.is_safe && modified_goal_pose_ && - (!prev_goal_id_ || *prev_goal_id_ != modified_goal_pose_->id)) { +void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +{ + if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } +} + +void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const +{ + // set the modified goal only when it is updated + const auto & route_handler = planner_data_->route_handler; + const bool has_changed_goal = + modified_goal_pose_ && (!prev_goal_id_ || *prev_goal_id_ != modified_goal_pose_->id); + if (status_.is_safe && has_changed_goal) { PoseWithUuidStamped modified_goal{}; - modified_goal.uuid = planner_data_->route_handler->getRouteUuid(); + modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose; - modified_goal.header = planner_data_->route_handler->getRouteHeader(); + modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; - prev_goal_id_ = modified_goal_pose_->id; } else { output.modified_goal = {}; } +} + +void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const +{ + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto new_signal = calcTurnSignalInfo(); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); +} +void GoalPlannerModule::updateSteeringFactor( + const std::array & pose, const std::array distance, const uint16_t type) +{ const uint16_t steering_factor_direction = std::invoke([this]() { const auto turn_signal = calcTurnSignalInfo(); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -714,28 +692,130 @@ BehaviorModuleOutput PullOverModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( + pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, ""); +} + +bool GoalPlannerModule::hasDecidedPath() const +{ + // once decided, keep the decision + if (status_.has_decided_path) { + return true; + } + + // if path is not safe, not decided + if (!status_.is_safe) { + return false; + } + + // if ego is sufficiently close to the start of the nearest candidate path, the path is decided + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + getCurrentPath().points, current_pose, std::numeric_limits::max(), M_PI_2); + if (!ego_segment_idx) { + return false; + } + const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( + getCurrentPath().points, status_.pull_over_path->start_pose.position); + const double dist_to_parking_start_pose = calcSignedArcLength( + getCurrentPath().points, current_pose.position, *ego_segment_idx, + status_.pull_over_path->start_pose.position, start_pose_segment_idx); + return dist_to_parking_start_pose < parameters_->decide_path_distance; +} + +void GoalPlannerModule::decideVelocity() +{ + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + + // decide velocity to guarantee turn signal lighting time + if (!status_.has_decided_velocity) { + auto & first_path = status_.pull_over_path->partial_paths.front(); + const auto vel = + static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); + for (auto & p : first_path.points) { + p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); + } + } + status_.has_decided_velocity = true; +} + +BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() +{ + constexpr double path_update_duration = 1.0; + + resetPathCandidate(); + resetPathReference(); + + // set current road lanes, pull over lanes, and drivable lane + setLanes(); + + // Check if it needs to decide path + status_.has_decided_path = hasDecidedPath(); + + // Use decided path + if (status_.has_decided_path) { + if (isActivated() && isWaitingApproval()) { + last_approved_time_ = std::make_unique(clock_->now()); + last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); + clearWaitingApproval(); + decideVelocity(); + } + transitionToNextPathIfFinishingCurrentPath(); + } else if (!pull_over_path_candidates_.empty() && needPathUpdate(path_update_duration)) { + // if the final path is not decided and enough time has passed since last path update, + // select safe path from lane parking pull over path candidates + // and set it to status_.pull_over_path + selectSafePullOverPath(); + } + // else: stop path is generated and set by setOutput() + + // set output and status + BehaviorModuleOutput output{}; + setOutput(output); + path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); + path_reference_ = getPreviousModuleOutput().reference_path; + + // return to lane parking if it is possible + if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + returnToLaneParking(); + } + + const auto distance_to_path_change = calcDistanceToPathChange(); + if (status_.has_decided_path) { + updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); + } + // TODO(tkhmy) add handle status TRYING + updateSteeringFactor( {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, - {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, - steering_factor_direction, SteeringFactor::TURNING, ""); + {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); - // For evaluations + // For debug + setDebugData(); if (parameters_->print_debug_info) { + // For evaluations printParkingPositionError(); } return output; } -// This const function can not change the menber variables like the goal. -// so implement generating candidate path in planWaitingApproval(). -// No specific path for the candidate. It's same to the one generated by plan(). -CandidateOutput PullOverModule::planCandidate() const +BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - return CandidateOutput{}; + if (allow_goal_modification_) { + return planWaitingApprovalWithGoalModification(); + } else { + // for fixed goals, only minor path refinements are made, + // so other modules are always allowed to run. + setIsSimultaneousExecutableAsApprovedModule(true); + setIsSimultaneousExecutableAsCandidateModule(true); + fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); + return fixed_goal_planner_->plan(planner_data_); + } } -BehaviorModuleOutput PullOverModule::planWaitingApproval() +BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() { + waitApproval(); + updateOccupancyGrid(); BehaviorModuleOutput out; out.modified_goal = plan().modified_goal; // update status_ @@ -746,28 +826,32 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() : out.path; path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); - updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); - const uint16_t steering_factor_direction = std::invoke([this]() { - const auto turn_signal = calcTurnSignalInfo(); - if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + // generate drivable area info for new architecture + if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + out.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes); - steering_factor_interface_ptr_->updateSteeringFactor( + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + out.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } + + if (status_.has_decided_path) { + updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); + } + updateSteeringFactor( {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, - {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, - steering_factor_direction, SteeringFactor::APPROACHING, ""); - waitApproval(); + {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); return out; } -std::pair PullOverModule::calcDistanceToPathChange() const +std::pair GoalPlannerModule::calcDistanceToPathChange() const { const auto & full_path = status_.pull_over_path->getFullPath(); @@ -792,12 +876,12 @@ std::pair PullOverModule::calcDistanceToPathChange() const return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } -void PullOverModule::setParameters(const std::shared_ptr & parameters) +void GoalPlannerModule::setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; } -PathWithLaneId PullOverModule::generateStopPath() +PathWithLaneId GoalPlannerModule::generateStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -858,17 +942,14 @@ PathWithLaneId PullOverModule::generateStopPath() // generate drivable area const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes); utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + reference_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_); return reference_path; } -PathWithLaneId PullOverModule::generateFeasibleStopPath() +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -896,17 +977,30 @@ PathWithLaneId PullOverModule::generateFeasibleStopPath() // generate drivable area const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(stop_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(stop_path, drivable_lanes); utils::generateDrivableArea( - stop_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + stop_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_); return stop_path; } -bool PullOverModule::incrementPathIndex() +void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() +{ + if (isActivated() && last_approved_time_ != nullptr) { + // if using arc_path and finishing current_path, get next path + // enough time for turn signal + const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > + planner_data_->parameters.turn_signal_search_time; + + if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { + if (incrementPathIndex()) { + last_increment_time_ = std::make_unique(clock_->now()); + } + } + } +} + +bool GoalPlannerModule::incrementPathIndex() { if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { return false; @@ -915,12 +1009,12 @@ bool PullOverModule::incrementPathIndex() return true; } -PathWithLaneId PullOverModule::getCurrentPath() const +PathWithLaneId GoalPlannerModule::getCurrentPath() const { return status_.pull_over_path->partial_paths.at(status_.current_path_idx); } -bool PullOverModule::isStopped( +bool GoalPlannerModule::isStopped( std::deque & odometry_buffer, const double time) { odometry_buffer.push_back(planner_data_->self_odometry); @@ -944,12 +1038,12 @@ bool PullOverModule::isStopped( return is_stopped; } -bool PullOverModule::isStopped() +bool GoalPlannerModule::isStopped() { return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } -bool PullOverModule::isStuck() +bool GoalPlannerModule::isStuck() { if (!status_.pull_over_path) { return false; @@ -958,7 +1052,7 @@ bool PullOverModule::isStuck() return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); } -bool PullOverModule::hasFinishedCurrentPath() +bool GoalPlannerModule::hasFinishedCurrentPath() { const auto & current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; @@ -968,17 +1062,20 @@ bool PullOverModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -bool PullOverModule::hasFinishedPullOver() +bool GoalPlannerModule::isOnGoal() const { - // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_odometry->pose.pose; - const bool car_is_on_goal = - calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < parameters_->th_arrived_distance; + const Pose current_pose = planner_data_->self_odometry->pose.pose; + const Pose goal_pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose + : planner_data_->route_handler->getGoalPose(); + return calcDistance2d(current_pose, goal_pose) < parameters_->th_arrived_distance; +} - return car_is_on_goal && isStopped(); +bool GoalPlannerModule::hasFinishedGoalPlanner() +{ + return isOnGoal() && isStopped(); } -TurnSignalInfo PullOverModule::calcTurnSignalInfo() const +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output @@ -1010,7 +1107,7 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const return turn_signal; } -bool PullOverModule::checkCollision(const PathWithLaneId & path) const +bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { const bool check_out_of_range = false; @@ -1030,7 +1127,7 @@ bool PullOverModule::checkCollision(const PathWithLaneId & path) const return false; } -bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) const +bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -1059,7 +1156,7 @@ bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) cons return true; } -void PullOverModule::keepStoppedWithCurrentPath(PathWithLaneId & path) +void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; constexpr double keep_current_idx_buffer_time = 2.0; @@ -1079,7 +1176,7 @@ void PullOverModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional PullOverModule::calcFeasibleDecelDistance( +boost::optional GoalPlannerModule::calcFeasibleDecelDistance( const double target_velocity) const { const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; @@ -1103,7 +1200,7 @@ boost::optional PullOverModule::calcFeasibleDecelDistance( return min_stop_distance; } -double PullOverModule::calcSignedArcLengthFromEgo( +double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1116,7 +1213,7 @@ double PullOverModule::calcSignedArcLengthFromEgo( path.points, current_pose.position, ego_idx, pose.position, target_idx); } -void PullOverModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const +void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const { const double time = planner_data_->parameters.turn_signal_search_time; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -1152,7 +1249,7 @@ void PullOverModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLan } } -void PullOverModule::decelerateBeforeSearchStart( +void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & search_start_pose, PathWithLaneId & path) const { const double pull_over_velocity = parameters_->pull_over_velocity; @@ -1168,7 +1265,105 @@ void PullOverModule::decelerateBeforeSearchStart( } } -void PullOverModule::setDebugData() +bool GoalPlannerModule::isCrossingPossible( + const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const +{ + if (start_lane.centerline().empty() || end_lane.centerline().empty()) { + return false; + } + + if (start_lane == end_lane) { + return true; + } + + const auto & route_handler = planner_data_->route_handler; + + lanelet::ConstLanelets start_lane_sequence = route_handler->getLaneletSequence(start_lane); + + // get end lane sequence based on whether it is shoulder lanelet or not + lanelet::ConstLanelets end_lane_sequence{}; + const bool is_shoulder_lane = route_handler->isShoulderLanelet(end_lane); + if (is_shoulder_lane) { + Pose end_lane_pose{}; + end_lane_pose.orientation.w = 1.0; + end_lane_pose.position = lanelet::utils::conversion::toGeomMsgPt(end_lane.centerline().front()); + end_lane_sequence = route_handler->getShoulderLaneletSequence(end_lane, end_lane_pose); + } else { + const double dist = std::numeric_limits::max(); + end_lane_sequence = route_handler->getLaneletSequence(end_lane, dist, dist, false); + } + + // Lambda function to get the neighboring lanelet based on left_side_parking_ + auto getNeighboringLane = + [&](const lanelet::ConstLanelet & lane) -> boost::optional { + lanelet::ConstLanelet neighboring_lane{}; + if (left_side_parking_) { + if (route_handler->getLeftShoulderLanelet(lane, &neighboring_lane)) { + return neighboring_lane; + } else { + return route_handler->getLeftLanelet(lane); + } + } else { + if (route_handler->getRightShoulderLanelet(lane, &neighboring_lane)) { + return neighboring_lane; + } else { + return route_handler->getRightLanelet(lane); + } + } + }; + + // Iterate through start_lane_sequence to find a path to end_lane_sequence + for (auto it = start_lane_sequence.rbegin(); it != start_lane_sequence.rend(); ++it) { + lanelet::ConstLanelet current_lane = *it; + + // Check if the current lane is in the end_lane_sequence + auto end_it = std::find(end_lane_sequence.rbegin(), end_lane_sequence.rend(), current_lane); + if (end_it != end_lane_sequence.rend()) { + return true; + } + + // Traversing is not allowed between road lanes + if (!is_shoulder_lane) { + continue; + } + + // Traverse the lanes horizontally until the end_lane_sequence is reached + boost::optional neighboring_lane = getNeighboringLane(current_lane); + if (neighboring_lane) { + // Check if the neighboring lane is in the end_lane_sequence + end_it = + std::find(end_lane_sequence.rbegin(), end_lane_sequence.rend(), neighboring_lane.get()); + if (end_it != end_lane_sequence.rend()) { + return true; + } + } + } + + return false; +} + +bool GoalPlannerModule::isCrossingPossible( + const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const +{ + lanelet::ConstLanelet start_lane{}; + lanelet::utils::query::getClosestLanelet(lanes, start_pose, &start_lane); + + lanelet::ConstLanelet end_lane{}; + lanelet::utils::query::getClosestLanelet(lanes, end_pose, &end_lane); + + return isCrossingPossible(start_lane, end_lane); +} + +bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const +{ + const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.lanes); + const Pose & start_pose = pull_over_path.start_pose; + const Pose & end_pose = pull_over_path.end_pose; + + return isCrossingPossible(start_pose, end_pose, lanes); +} + +void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); @@ -1185,16 +1380,16 @@ void PullOverModule::setDebugData() tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (parameters_->enable_goal_research) { + if (allow_goal_modification_) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = refined_goal_pose_.position.z; - add(pull_over_utils::createPullOverAreaMarkerArray( + add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - add(pull_over_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); } // Visualize path and related pose @@ -1252,7 +1447,7 @@ void PullOverModule::setDebugData() } } -void PullOverModule::printParkingPositionError() const +void GoalPlannerModule::printParkingPositionError() const { const auto current_pose = planner_data_->self_odometry->pose.pose; const double real_shoulder_to_map_shoulder = 0.0; @@ -1269,4 +1464,32 @@ void PullOverModule::printParkingPositionError() const tf2::getYaw(modified_goal_pose_->goal_pose.orientation)), distance_from_real_shoulder); } + +bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const +{ + const auto & route_handler = planner_data_->route_handler; + const Pose & goal_pose = route_handler->getGoalPose(); + + const lanelet::ConstLanelets pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + lanelet::ConstLanelet target_lane{}; + lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + + return route_handler->isShoulderLanelet(target_lane) && + lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); +} + +bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const +{ + return !isOnGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); +} + +bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const +{ + if (!last_path_update_time_) { + return true; + } + + return (clock_->now() - *last_path_update_time_).seconds() > duration; +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp similarity index 84% rename from planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp rename to planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 63903745eadd2..af96313aa0c19 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/pull_over/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include @@ -23,14 +23,14 @@ namespace behavior_path_planner { -PullOverModuleManager::PullOverModuleManager( +GoalPlannerModuleManager::GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters) : SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} { } -void PullOverModuleManager::updateModuleParams( +void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { using tier4_autoware_utils::updateParam; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp new file mode 100644 index 0000000000000..298f2c8db3ad8 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -0,0 +1,225 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" + +#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_utils.hpp" + +#include + +#include + +namespace behavior_path_planner +{ +AvoidanceByLaneChange::AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters) +: NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) +{ +} + +std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_path) const +{ + const auto & avoidance_objects = avoidance_data_.target_objects; + const auto execute_object_num = avoidance_by_lane_change_parameters_->execute_object_num; + + if (avoidance_objects.size() < execute_object_num) { + return {false, false}; + } + + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return {false, false}; + } + + const auto & o_front = avoidance_objects.front(); + + // check distance from ego to o_front vs acceptable longitudinal margin + const auto execute_object_longitudinal_margin = + avoidance_by_lane_change_parameters_->execute_object_longitudinal_margin; + if (execute_object_longitudinal_margin > o_front.longitudinal) { + return {false, false}; + } + + const auto direction = utils::avoidance::isOnRight(o_front) ? Direction::LEFT : Direction::RIGHT; + const auto target_lanes = getLaneChangeLanes(current_lanes, direction); + + if (target_lanes.empty()) { + return {false, false}; + } + + // find candidate paths + LaneChangePaths valid_paths{}; + const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction, &valid_paths); + + if (valid_paths.empty()) { + return {false, false}; + } + + if (found_safe_path) { + safe_path = valid_paths.back(); + } else { + // force candidate + safe_path = valid_paths.front(); + } + + const auto to_lane_change_end_distance = motion_utils::calcSignedArcLength( + safe_path.path.points, getEgoPose().position, safe_path.shift_line.end.position); + const auto lane_change_finish_before_object = o_front.longitudinal > to_lane_change_end_distance; + const auto execute_only_when_lane_change_finish_before_object = + avoidance_by_lane_change_parameters_->execute_only_when_lane_change_finish_before_object; + if (!lane_change_finish_before_object && execute_only_when_lane_change_finish_before_object) { + return {false, found_safe_path}; + } + return {true, found_safe_path}; +} + +void AvoidanceByLaneChange::updateSpecialData() +{ + avoidance_debug_data_ = DebugData(); + avoidance_data_ = calcAvoidancePlanningData(avoidance_debug_data_); + + utils::avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, + avoidance_by_lane_change_parameters_->avoidance); + utils::avoidance::compensateDetectionLost( + registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + + std::sort( + avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), + [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); +} + +AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( + AvoidanceDebugData & debug) const +{ + AvoidancePlanningData data; + + // reference pose + data.reference_pose = getEgoPose(); + + data.reference_path_rough = prev_module_path_; + + const auto resample_interval = + avoidance_by_lane_change_parameters_->avoidance->resample_interval_for_planning; + data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); + + data.current_lanelets = getCurrentLanes(); + + // get related objects from dynamic_objects, and then separates them as target objects and non + // target objects + fillAvoidanceTargetObjects(data, debug); + + return data; +} + +void AvoidanceByLaneChange::fillAvoidanceTargetObjects( + AvoidancePlanningData & data, DebugData & debug) const +{ + const auto left_expand_dist = avoidance_parameters_->detection_area_left_expand_dist; + const auto right_expand_dist = avoidance_parameters_->detection_area_right_expand_dist; + + const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( + data.current_lanelets, left_expand_dist, right_expand_dist * (-1.0)); + + const auto [object_within_target_lane, object_outside_target_lane] = + utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + // Assume that the maximum allocation for data.other object is the sum of + // objects_within_target_lane and object_outside_target_lane. The maximum allocation for + // data.target_objects is equal to object_within_target_lane + { + const auto other_objects_size = + object_within_target_lane.objects.size() + object_outside_target_lane.objects.size(); + data.other_objects.reserve(other_objects_size); + data.target_objects.reserve(object_within_target_lane.objects.size()); + } + + { + const auto & objects = object_outside_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(data.other_objects), + [](const auto & object) { + ObjectData other_object; + other_object.object = object; + other_object.reason = "OutOfTargetArea"; + return other_object; + }); + } + + ObjectDataArray target_lane_objects; + target_lane_objects.reserve(object_within_target_lane.objects.size()); + { + const auto & objects = object_within_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(target_lane_objects), + [&](const auto & object) { return createObjectData(data, object); }); + } + + utils::avoidance::filterTargetObjects( + target_lane_objects, data, debug, planner_data_, avoidance_parameters_); +} + +ObjectData AvoidanceByLaneChange::createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const +{ + using boost::geometry::return_centroid; + + const auto & path_points = data.reference_path.points; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + motion_utils::findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; + + ObjectData object_data{}; + + object_data.object = object; + + // Calc envelop polygon. + utils::avoidance::fillObjectEnvelopePolygon( + object_data, registered_objects_, object_closest_pose, avoidance_parameters_); + + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); + + // Calc moving time. + utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, avoidance_parameters_); + + // Calc lateral deviation from path to target object. + object_data.lateral = + tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( + object_data, object_closest_pose, object_data.overhang_pose.position); + + // Check whether the the ego should avoid the object. + const auto t = utils::getHighestProbLabel(object.classification); + const auto object_parameter = avoidance_parameters_->object_parameters.at(t); + const auto vehicle_width = planner_data_->parameters.vehicle_width; + const auto safety_margin = 0.5 * vehicle_width + object_parameter.safety_buffer_lateral; + object_data.avoid_required = + (utils::avoidance::isOnRight(object_data) && + std::abs(object_data.overhang_dist) < safety_margin) || + (!utils::avoidance::isOnRight(object_data) && object_data.overhang_dist < safety_margin); + + return object_data; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp index affeeee7bd9e8..110a993214ae4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp @@ -31,38 +31,13 @@ namespace behavior_path_planner { ExternalRequestLaneChange::ExternalRequestLaneChange( const std::shared_ptr & parameters, Direction direction) -: NormalLaneChange(parameters, direction) +: NormalLaneChange(parameters, LaneChangeModuleType::EXTERNAL_REQUEST, direction) { } -lanelet::ConstLanelets ExternalRequestLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const +ExternalRequestLaneChangeBT::ExternalRequestLaneChangeBT( + const std::shared_ptr & parameters, Direction direction) +: NormalLaneChangeBT(parameters, LaneChangeModuleType::EXTERNAL_REQUEST, direction) { - if (current_lanes.empty()) { - return {}; - } - // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, getEgoPose(), ¤t_lane); - - const auto minimum_lane_changing_length = planner_data_->parameters.minimum_lane_changing_length; - - const auto lane_change_prepare_length = - std::max(getEgoVelocity() * parameters_->prepare_duration, minimum_lane_changing_length); - - const auto & route_handler = getRouteHandler(); - - const auto current_check_lanes = - route_handler->getLaneletSequence(current_lane, getEgoPose(), 0.0, lane_change_prepare_length); - - const auto lane_change_lane = - route_handler->getLaneChangeTargetExceptPreferredLane(current_check_lanes, direction_); - - if (lane_change_lane) { - return route_handler->getLaneletSequence( - lane_change_lane.get(), getEgoPose(), lane_change_lane_length_, lane_change_lane_length_); - } - - return {}; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp deleted file mode 100644 index c44d46c4ba9a0..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ /dev/null @@ -1,757 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp" - -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -namespace behavior_path_planner -{ -#ifdef USE_OLD_ARCHITECTURE -ExternalRequestLaneChangeModule::ExternalRequestLaneChangeModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const Direction & direction) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, - parameters_{std::move(parameters)}, - direction_{direction} -{ -} - -void ExternalRequestLaneChangeModule::processOnEntry() -{ - current_lane_change_state_ = LaneChangeStates::Normal; - updateLaneChangeStatus(); -} - -void ExternalRequestLaneChangeModule::processOnExit() -{ - resetParameters(); -} - -bool ExternalRequestLaneChangeModule::isExecutionRequested() const -{ - if (current_state_ == BT::NodeStatus::RUNNING) { - return true; - } - - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_valid_path; -} - -bool ExternalRequestLaneChangeModule::isExecutionReady() const -{ - if (current_state_ == BT::NodeStatus::RUNNING) { - return true; - } - - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_safe_path; -} - -BT::NodeStatus ExternalRequestLaneChangeModule::updateState() -{ - RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; - return current_state_; - } - - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (isAbortState() && !is_within_current_lane) { - current_state_ = BT::NodeStatus::RUNNING; - return current_state_; - } - - if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentVelocityLow()) { - current_state_ = BT::NodeStatus::RUNNING; - return current_state_; - } - - current_state_ = BT::NodeStatus::FAILURE; - return current_state_; - } - - if (hasFinishedLaneChange()) { - current_state_ = BT::NodeStatus::SUCCESS; - return current_state_; - } - current_state_ = BT::NodeStatus::RUNNING; - return current_state_; -} - -BehaviorModuleOutput ExternalRequestLaneChangeModule::plan() -{ - resetPathCandidate(); - is_activated_ = isActivated(); - - auto path = status_.lane_change_path.path; - - if (!isValidPath(path)) { - status_.is_safe = false; - return BehaviorModuleOutput{}; - } - - if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentVelocityLow())) { - const auto stop_point = utils::insertStopPoint(0.1, path); - } - - if (isAbortState()) { - resetPathIfAbort(); - if (is_activated_) { - path = abort_path_->path; - } - } - - generateExtendedDrivableArea(path); - - prev_approved_path_ = path; - - BehaviorModuleOutput output; - output.path = std::make_shared(path); - updateOutputTurnSignal(output); - - updateSteeringFactorPtr(output); - - return output; -} - -void ExternalRequestLaneChangeModule::resetPathIfAbort() -{ - if (!abort_path_) { - RCLCPP_WARN(getLogger(), "[abort] No abort path found!"); - } - - if (!is_abort_approval_requested_) { - RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); - } - is_abort_approval_requested_ = true; - is_abort_path_approved_ = false; - return; - } - - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); - is_abort_path_approved_ = true; - clearWaitingApproval(); - } else { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); - is_abort_path_approved_ = false; - const double start_distance = motion_utils::calcSignedArcLength( - abort_path_->path.points, getEgoPose().position, abort_path_->shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - abort_path_->path.points, getEgoPose().position, abort_path_->shift_line.end.position); - waitApproval(start_distance, finish_distance); - } -} - -CandidateOutput ExternalRequestLaneChangeModule::planCandidate() const -{ - CandidateOutput output; - - // Get lane change lanes - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - - if (isAbortState()) { - selected_path = *abort_path_; - } - - if (selected_path.path.points.empty()) { - return output; - } - - output.path_candidate = selected_path.path; - output.lateral_shift = utils::lane_change::getLateralShift(selected_path); - output.start_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - - updateSteeringFactorPtr(output, selected_path); - return output; -} - -BehaviorModuleOutput ExternalRequestLaneChangeModule::planWaitingApproval() -{ - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - - if (is_within_current_lane) { - prev_approved_path_ = getReferencePath(); - } - - BehaviorModuleOutput out; - out.path = std::make_shared(getReferencePath()); - - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - waitApproval(candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); - is_abort_path_approved_ = false; - return out; -} - -void ExternalRequestLaneChangeModule::updateLaneChangeStatus() -{ - status_.current_lanes = utils::getCurrentLanes(planner_data_); - status_.lane_change_lanes = getLaneChangeLanes(status_.current_lanes, lane_change_lane_length_); - - // Find lane change path - LaneChangePath selected_path; - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - getSafePath(status_.lane_change_lanes, check_distance_, selected_path); - - // Update status - status_.is_safe = found_safe_path; - status_.lane_change_path = selected_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.lane_change_lanes); - - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, getEgoPose()); - status_.start_distance = arclength_start.length; - status_.lane_change_path.path.header = getRouteHeader(); -} - -PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const -{ - PathWithLaneId reference_path; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto & common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = getRouteHeader(); - - const auto current_lanes = utils::getCurrentLanes(planner_data_); - - if (current_lanes.empty()) { - return reference_path; - } - - if (reference_path.points.empty()) { - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); - } - - const int num_lane_change = - std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, 0.0); - - const double lane_change_buffer = - utils::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); - - reference_path = utils::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_->prepare_duration, - lane_change_buffer); - - const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); - utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); - - return reference_path; -} - -lanelet::ConstLanelets ExternalRequestLaneChangeModule::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const -{ - lanelet::ConstLanelets lane_change_lanes; - const auto & route_handler = planner_data_->route_handler; - const auto minimum_lane_changing_length = planner_data_->parameters.minimum_lane_changing_length; - const auto prepare_duration = parameters_->prepare_duration; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - - if (current_lanes.empty()) { - return lane_change_lanes; - } - - // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); - const double lane_change_prepare_length = - std::max(current_twist.linear.x * prepare_duration, minimum_lane_changing_length); - lanelet::ConstLanelets current_check_lanes = - route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); - lanelet::ConstLanelet lane_change_lane; - - auto getLaneChangeTargetExceptPreferredLane = [this, &route_handler]( - const lanelet::ConstLanelets & lanelets, - lanelet::ConstLanelet * target_lanelet) { - return direction_ == Direction::RIGHT - ? route_handler->getRightLaneChangeTargetExceptPreferredLane(lanelets, target_lanelet) - : route_handler->getLeftLaneChangeTargetExceptPreferredLane(lanelets, target_lanelet); - }; - - if (getLaneChangeTargetExceptPreferredLane(current_check_lanes, &lane_change_lane)) { - lane_change_lanes = route_handler->getLaneletSequence( - lane_change_lane, current_pose, lane_change_lane_length, lane_change_lane_length); - } else { - lane_change_lanes.clear(); - } - - return lane_change_lanes; -} - -std::pair ExternalRequestLaneChangeModule::getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & common_parameters = planner_data_->parameters; - - const auto current_lanes = utils::getCurrentLanes(planner_data_); - - if (lane_change_lanes.empty()) { - return std::make_pair(false, false); - } - - // find candidate paths - LaneChangePaths valid_paths; - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths, - &object_debug_); - debug_valid_path_ = valid_paths; - - if (parameters_->publish_debug_marker) { - setObjectDebugVisualization(); - } else { - debug_marker_.markers.clear(); - } - - if (valid_paths.empty()) { - return {false, false}; - } - - if (found_safe_path) { - safe_path = valid_paths.back(); - } else { - // force candidate - safe_path = valid_paths.front(); - } - - return {true, found_safe_path}; -} - -bool ExternalRequestLaneChangeModule::isSafe() const -{ - return status_.is_safe; -} - -bool ExternalRequestLaneChangeModule::isValidPath(const PathWithLaneId & path) const -{ - const auto & route_handler = planner_data_->route_handler; - - // check lane departure - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.lane_change_lanes)); - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); - const auto lanelets = utils::transformToLanelets(expanded_lanes); - - // check path points are in any lanelets - for (const auto & point : path.points) { - bool is_in_lanelet = false; - for (const auto & lanelet : lanelets) { - if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { - is_in_lanelet = true; - break; - } - } - if (!is_in_lanelet) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; - } - } - - // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); - return false; - } - - return true; -} - -bool ExternalRequestLaneChangeModule::isNearEndOfLane() const -{ - const auto & current_pose = getEgoPose(); - const auto minimum_lane_changing_length = planner_data_->parameters.minimum_lane_changing_length; - const auto end_of_lane_buffer = planner_data_->parameters.backward_length_buffer_for_end_of_lane; - const double threshold = end_of_lane_buffer + minimum_lane_changing_length; - - return std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < - threshold; -} - -bool ExternalRequestLaneChangeModule::isCurrentVelocityLow() const -{ - constexpr double threshold_ms = 10.0 * 1000 / 3600; - return getEgoTwist().linear.x < threshold_ms; -} - -bool ExternalRequestLaneChangeModule::isAbortConditionSatisfied() -{ - is_abort_condition_satisfied_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - - if (!parameters_->enable_cancel_lane_change) { - return false; - } - - if (!is_activated_) { - return false; - } - - Pose ego_pose_before_collision; - const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - - if (!is_path_safe) { - const auto & common_parameters = planner_data_->parameters; - const bool is_within_original_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), common_parameters); - - if (is_within_original_lane) { - current_lane_change_state_ = LaneChangeStates::Cancel; - return true; - } - - // check abort enable flag - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); - - if (!parameters_->enable_abort_lane_change) { - current_lane_change_state_ = LaneChangeStates::Stop; - return false; - } - - const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, - *parameters_); - - if (!found_abort_path && !is_abort_path_approved_) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - - current_lane_change_state_ = LaneChangeStates::Abort; - - if (!is_abort_path_approved_) { - abort_path_ = std::make_shared(*found_abort_path); - } - - return true; - } - - return false; -} - -bool ExternalRequestLaneChangeModule::isAbortState() const -{ - if (!parameters_->enable_abort_lane_change) { - return false; - } - - if (current_lane_change_state_ != LaneChangeStates::Abort) { - return false; - } - - if (!abort_path_) { - return false; - } - - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); - return true; -} - -bool ExternalRequestLaneChangeModule::hasFinishedLaneChange() const -{ - const auto & current_pose = getEgoPose(); - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); - const double travel_distance = arclength_current.length - status_.start_distance; - const double finish_distance = - status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer; - return travel_distance > finish_distance; -} - -void ExternalRequestLaneChangeModule::setObjectDebugVisualization() const -{ - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; - - debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showObjectInfo(object_debug_, "object_debug_info")); - add(showLerpedPose(object_debug_, "lerp_pose_before_true")); - add(showPolygonPose(object_debug_, "expected_pose")); - add(showPolygon(object_debug_, "lerped_polygon")); - add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths")); -} - -std::shared_ptr ExternalRequestLaneChangeModule::get_debug_msg_array() - const -{ - LaneChangeDebugMsgArray debug_msg_array; - debug_msg_array.lane_change_info.reserve(object_debug_.size()); - for (const auto & [uuid, debug_data] : object_debug_) { - LaneChangeDebugMsg debug_msg; - debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; - debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; - debug_msg_array.lane_change_info.push_back(debug_msg); - } - lane_change_debug_msg_array_ = debug_msg_array; - - lane_change_debug_msg_array_.header.stamp = clock_->now(); - return std::make_shared(lane_change_debug_msg_array_); -} - -void ExternalRequestLaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) -{ - const auto current_pose = getEgoPose(); - const double start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); - - waitApproval(start_distance, finish_distance); - uint16_t steering_factor_direction; - if (direction_ == Direction::RIGHT) { - steering_factor_direction = SteeringFactor::RIGHT; - } else if (direction_ == Direction::LEFT) { - steering_factor_direction = SteeringFactor::LEFT; - } else { - steering_factor_direction = SteeringFactor::UNKNOWN; - } - - // TODO(tkhmy) add handle status TRYING - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.lane_change_path.shift_line.start, status_.lane_change_path.shift_line.end}, - {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, - SteeringFactor::TURNING, ""); -} - -void ExternalRequestLaneChangeModule::updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const -{ - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - - steering_factor_interface_ptr_->updateSteeringFactor( - {selected_path.shift_line.start, selected_path.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); -} -std_msgs::msg::Header ExternalRequestLaneChangeModule::getRouteHeader() const -{ - return planner_data_->route_handler->getRouteHeader(); -} -void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) -{ - const auto & common_parameters = planner_data_->parameters; - const auto & route_handler = planner_data_->route_handler; - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, status_.current_lanes, status_.lane_change_lanes); - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); - - utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); -} - -bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const -{ - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = *parameters_; - const auto & route_handler = planner_data_->route_handler; - const auto path = status_.lane_change_path; - - constexpr double check_distance = 100.0; - // get lanes used for detection - const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - *route_handler, path.target_lanelets.front(), current_pose, check_distance); - - std::unordered_map debug_data; - - const auto lateral_buffer = - utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); - const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, - lane_change_parameters, lateral_buffer); - - return utils::lane_change::isLaneChangePathSafe( - path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, - status_.lane_change_path.acceleration); -} - -void ExternalRequestLaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) -{ - calcTurnSignalInfo(); - const auto turn_signal_info = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - output.turn_signal_info.turn_signal.command = turn_signal_info.first.command; - - utils::lane_change::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); -} - -void ExternalRequestLaneChangeModule::calcTurnSignalInfo() -{ - const auto get_blinker_pose = - [this](const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double length) { - const auto & points = path.points; - const auto arc_front = lanelet::utils::getArcCoordinates(lanes, points.front().point.pose); - for (const auto & point : points) { - const auto & pt = point.point.pose; - const auto arc_current = lanelet::utils::getArcCoordinates(lanes, pt); - const auto diff = arc_current.length - arc_front.length; - if (diff > length) { - return pt; - } - } - - RCLCPP_WARN(getLogger(), "unable to determine blinker pose..."); - return points.front().point.pose; - }; - - const auto & path = status_.lane_change_path; - TurnSignalInfo turn_signal_info{}; - - turn_signal_info.desired_start_point = std::invoke([&]() { - const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto prepare_duration = parameters_->prepare_duration; - const auto prepare_to_blinker_start_diff = prepare_duration - blinker_start_duration; - if (prepare_to_blinker_start_diff < 1e-5) { - return path.path.points.front().point.pose; - } - - return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff); - }); - turn_signal_info.desired_end_point = path.shift_line.end; - - turn_signal_info.required_start_point = path.shift_line.start; - const auto mid_lane_change_length = path.length.lane_changing / 2; - const auto & shifted_path = path.shifted_path.path; - turn_signal_info.required_end_point = - get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length); - - status_.lane_change_path.turn_signal_info = turn_signal_info; -} - -void ExternalRequestLaneChangeModule::resetParameters() -{ - resetPathCandidate(); - object_debug_.clear(); - debug_marker_.markers.clear(); -} - -void ExternalRequestLaneChangeModule::acceptVisitor( - const std::shared_ptr & visitor) const -{ - if (visitor) { - visitor->visitExternalRequestLaneChangeModule(this); - } -} - -void SceneModuleVisitor::visitExternalRequestLaneChangeModule( - const ExternalRequestLaneChangeModule * module) const -{ - ext_request_lane_change_visitor_ = module->get_debug_msg_array(); -} - -ExternalRequestLaneChangeRightModule::ExternalRequestLaneChangeRightModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) -: ExternalRequestLaneChangeModule{name, node, parameters, Direction::RIGHT} -{ -} - -ExternalRequestLaneChangeLeftModule::ExternalRequestLaneChangeLeftModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) -: ExternalRequestLaneChangeModule{name, node, parameters, Direction::LEFT} -{ -} -#endif - -} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 02dcb07a1799f..fd632b0f49d51 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_planner/module_status.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" @@ -27,31 +28,32 @@ namespace behavior_path_planner { -using utils::lane_change::getLateralShift; +using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map > & rtc_interface_ptr_map, std::unique_ptr && module_type) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, - parameters_{parameters}, - module_type_{std::move(module_type)} + parameters_{std::move(parameters)}, + module_type_{std::move(module_type)}, + prev_approved_path_{std::make_unique()} { - prev_approved_path_ = std::make_unique(); steering_factor_interface_ptr_ = std::make_unique(&node, name); } void LaneChangeInterface::processOnEntry() { waitApproval(); - module_type_->updateLaneChangeStatus( - *getPreviousModuleOutput().reference_path, *getPreviousModuleOutput().path); + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->updateLaneChangeStatus(); } void LaneChangeInterface::processOnExit() { module_type_->resetParameters(); + debug_marker_.markers.clear(); resetPathCandidate(); } @@ -62,21 +64,19 @@ bool LaneChangeInterface::isExecutionRequested() const } LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath( - *getPreviousModuleOutput().reference_path, *getPreviousModuleOutput().path, selected_path); + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); return found_valid_path; } bool LaneChangeInterface::isExecutionReady() const { - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath( - *getPreviousModuleOutput().reference_path, *getPreviousModuleOutput().path, selected_path); + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); return found_safe_path; } @@ -84,27 +84,105 @@ bool LaneChangeInterface::isExecutionReady() const ModuleStatus LaneChangeInterface::updateState() { if (!module_type_->isValidPath()) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; + return ModuleStatus::FAILURE; } if (module_type_->isAbortState()) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; + return module_type_->hasFinishedAbort() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING; } - if (module_type_->isCancelConditionSatisfied()) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; + if (module_type_->hasFinishedLaneChange()) { + return ModuleStatus::SUCCESS; } - if (module_type_->hasFinishedLaneChange()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; + const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); + + if (is_safe) { + module_type_->toNormalState(); + return ModuleStatus::RUNNING; + } + + if (!module_type_->isCancelEnabled()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe but cancel was not enabled. Continue lane change."); + if (module_type_->isRequiredStop(is_object_coming_from_rear)) { + module_type_->toStopState(); + } else { + module_type_->toNormalState(); + } + return ModuleStatus::RUNNING; + } + + if (!module_type_->isAbleToReturnCurrentLane()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe but cannot return. Continue lane change."); + if (module_type_->isRequiredStop(is_object_coming_from_rear)) { + module_type_->toStopState(); + } else { + module_type_->toNormalState(); + } + return ModuleStatus::RUNNING; + } + + if (module_type_->isNearEndOfLane()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe but near end of lane. Continue lane change."); + if (module_type_->isRequiredStop(is_object_coming_from_rear)) { + module_type_->toStopState(); + } else { + module_type_->toNormalState(); + } + return ModuleStatus::RUNNING; } - current_state_ = ModuleStatus::RUNNING; - return current_state_; + if (module_type_->isEgoOnPreparePhase()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe. Cancel lane change."); + module_type_->toCancelState(); + return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::FAILURE; + } + + if (!module_type_->isAbortEnabled()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe but abort was not enabled. Continue lane change."); + if (module_type_->isRequiredStop(is_object_coming_from_rear)) { + module_type_->toStopState(); + } else { + module_type_->toNormalState(); + } + return ModuleStatus::RUNNING; + } + + const auto found_abort_path = module_type_->getAbortPath(); + if (!found_abort_path) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe but not found abort path. Continue lane change."); + if (module_type_->isRequiredStop(is_object_coming_from_rear)) { + module_type_->toStopState(); + } else { + module_type_->toNormalState(); + } + return ModuleStatus::RUNNING; + } + + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Lane change path is unsafe. Abort lane change."); + module_type_->toAbortState(); + return ModuleStatus::RUNNING; +} + +void LaneChangeInterface::updateData() +{ + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->updateSpecialData(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -112,25 +190,16 @@ BehaviorModuleOutput LaneChangeInterface::plan() resetPathCandidate(); resetPathReference(); - const auto path = module_type_->generatePlannedPath(getPreviousModuleOutput().drivable_lanes); - if (!module_type_->isValidPath()) { return {}; } - if (module_type_->isAbortState()) { - resetPathIfAbort(); - } - - const auto reference_path = module_type_->getReferencePath(); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); - output.reference_path = std::make_shared(reference_path); - - path_reference_ = std::make_shared(reference_path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); + auto output = module_type_->generateOutput(); + path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; - output.turn_signal_info = module_type_->updateOutputTurnSignal(); + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -145,9 +214,11 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.path = std::make_shared(*getPreviousModuleOutput().path); out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - module_type_->updateLaneChangeStatus( - *getPreviousModuleOutput().reference_path, *prev_approved_path_); + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->updateLaneChangeStatus(); if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); @@ -173,38 +244,12 @@ CandidateOutput LaneChangeInterface::planCandidate() const return {}; } - CandidateOutput output; - output.path_candidate = selected_path.path; - output.lateral_shift = getLateralShift(selected_path); - output.start_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, module_type_->getEgoPosition(), - selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, module_type_->getEgoPosition(), - selected_path.shift_line.end.position); + CandidateOutput output = assignToCandidate(selected_path, module_type_->getEgoPosition()); updateSteeringFactorPtr(output, selected_path); return output; } -void LaneChangeInterface::resetPathIfAbort() -{ - if (!is_abort_approval_requested_) { - removeRTCStatus(); - is_abort_approval_requested_ = true; - clearAbortApproval(); - return; - } - - if (isActivated()) { - is_abort_path_approved_ = true; - clearWaitingApproval(); - } else { - clearAbortApproval(); - waitApproval(); - } -} - void LaneChangeInterface::updateModuleParams( const std::shared_ptr & parameters) { @@ -262,13 +307,6 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { - const auto current_pose = module_type_->getEgoPose(); - const auto status = module_type_->getLaneChangeStatus(); - const double start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status.lane_change_path.shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status.lane_change_path.shift_line.end.position); - const auto steering_factor_direction = std::invoke([&]() { if (module_type_->getDirection() == Direction::LEFT) { return SteeringFactor::LEFT; @@ -279,6 +317,13 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o return SteeringFactor::UNKNOWN; }); + const auto current_position = module_type_->getEgoPosition(); + const auto status = module_type_->getLaneChangeStatus(); + const auto start_distance = motion_utils::calcSignedArcLength( + output.path->points, current_position, status.lane_change_path.shift_line.start.position); + const auto finish_distance = motion_utils::calcSignedArcLength( + output.path->points, current_position, status.lane_change_path.shift_line.end.position); + // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.shift_line.start, status.lane_change_path.shift_line.end}, @@ -312,4 +357,159 @@ void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * in { lane_change_visitor_ = interface->get_debug_msg_array(); } + +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map > & rtc_interface_ptr_map) +: LaneChangeInterface{ + name, node, parameters, rtc_interface_ptr_map, + std::make_unique( + parameters, avoidance_parameters, avoidance_by_lane_change_parameters)} +{ +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + +LaneChangeBTInterface::LaneChangeBTInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map > & rtc_interface_ptr_map, + std::unique_ptr && module_type) +: LaneChangeInterface{name, node, parameters, rtc_interface_ptr_map, std::move(module_type)} +{ +} + +void LaneChangeBTInterface::processOnEntry() +{ + module_type_->updateLaneChangeStatus(); +} + +BehaviorModuleOutput LaneChangeBTInterface::plan() +{ + resetPathCandidate(); + resetPathReference(); + is_activated_ = isActivated(); + + if (!module_type_->isValidPath()) { + return {}; + } + + auto output = module_type_->generateOutput(); + path_reference_ = getPreviousModuleOutput().reference_path; + *prev_approved_path_ = *output.path; + + updateSteeringFactorPtr(output); + clearWaitingApproval(); + + return output; +} + +BehaviorModuleOutput LaneChangeBTInterface::planWaitingApproval() +{ + const auto path = module_type_->getReferencePath(); + if (!path.points.empty()) { + *prev_approved_path_ = module_type_->getReferencePath(); + } + + BehaviorModuleOutput out; + out.path = std::make_shared(*prev_approved_path_); + out.reference_path = getPreviousModuleOutput().reference_path; + out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + + path_reference_ = getPreviousModuleOutput().reference_path; + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + is_abort_path_approved_ = false; + + return out; +} + +CandidateOutput LaneChangeBTInterface::planCandidate() const +{ + LaneChangePath selected_path; + + if (module_type_->isAbortState()) { + selected_path = module_type_->getLaneChangePath(); + } else { + [[maybe_unused]] const auto [found_valid_path, found_safe_path] = + module_type_->getSafePath(selected_path); + } + + selected_path.path.header = module_type_->getRouteHeader(); + + if (selected_path.path.points.empty()) { + return {}; + } + + CandidateOutput output = assignToCandidate(selected_path, module_type_->getEgoPosition()); + + updateSteeringFactorPtr(output, selected_path); + return output; +} + +void LaneChangeBTInterface::acceptVisitor(const std::shared_ptr & visitor) const +{ + if (visitor) { + visitor->visitLaneChangeBTInterface(this); + } +} + +void SceneModuleVisitor::visitLaneChangeBTInterface(const LaneChangeBTInterface * module) const +{ + external_request_lane_change_bt_visitor_ = module->get_debug_msg_array(); +} + +LaneChangeBTModule::LaneChangeBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters) +: LaneChangeBTInterface{ + name, node, parameters, createRTCInterfaceMap(node, name, {"left", "right"}), + std::make_unique(parameters, LaneChangeModuleType::NORMAL, Direction::NONE)} +{ +} + +void LaneChangeBTModule::updateRTCStatus(const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + +ExternalRequestLaneChangeLeftBTModule::ExternalRequestLaneChangeLeftBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters) +: LaneChangeBTInterface{ + name, node, parameters, createRTCInterfaceMap(node, name, {""}), + std::make_unique(parameters, Direction::LEFT)} +{ +} + +ExternalRequestLaneChangeRightBTModule::ExternalRequestLaneChangeRightBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters) +: LaneChangeBTInterface{ + name, node, parameters, createRTCInterfaceMap(node, name, {""}), + std::make_unique(parameters, Direction::RIGHT)} +{ +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp deleted file mode 100644 index 864710bf43c23..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ /dev/null @@ -1,867 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" - -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_perception_msgs::msg::ObjectClassification; - -#ifdef USE_OLD_ARCHITECTURE -LaneChangeModule::LaneChangeModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {"left", "right"})}, - parameters_{std::move(parameters)} -#else -LaneChangeModule::LaneChangeModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map, - Direction direction, LaneChangeModuleType type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, - parameters_{parameters}, - direction_{direction}, - type_{type} -#endif -{ -} - -void LaneChangeModule::processOnEntry() -{ -#ifndef USE_OLD_ARCHITECTURE - waitApproval(); -#endif - current_lane_change_state_ = LaneChangeStates::Normal; - updateLaneChangeStatus(); -} - -void LaneChangeModule::processOnExit() -{ - resetParameters(); -} - -bool LaneChangeModule::isExecutionRequested() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); - const auto lane_change_lanes = utils::lane_change::getLaneChangeLanes( - planner_data_, current_lanes, lane_change_lane_length_, parameters_->prepare_duration, - direction_, type_); -#endif - - if (lane_change_lanes.empty()) { - return false; - } - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_valid_path; -} - -bool LaneChangeModule::isExecutionReady() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); - const auto lane_change_lanes = utils::lane_change::getLaneChangeLanes( - planner_data_, current_lanes, lane_change_lane_length_, parameters_->prepare_duration, - direction_, type_); -#endif - - if (lane_change_lanes.empty()) { - return false; - } - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_safe_path; -} - -ModuleStatus LaneChangeModule::updateState() -{ - RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); - if (!isValidPath()) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (isAbortState() && !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentVelocityLow()) || !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - if (hasFinishedLaneChange()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - current_state_ = ModuleStatus::RUNNING; - return current_state_; -} - -BehaviorModuleOutput LaneChangeModule::plan() -{ - resetPathCandidate(); - resetPathReference(); - is_activated_ = isActivated(); - - PathWithLaneId path = status_.lane_change_path.path; - if (!isValidPath(path)) { - status_.is_valid_path = false; - return BehaviorModuleOutput{}; - } else { - status_.is_valid_path = true; - } - - if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentVelocityLow())) { - const auto stop_point = utils::insertStopPoint(0.1, path); - } - - if (isAbortState()) { - resetPathIfAbort(); - if (is_activated_) { - path = abort_path_->path; - } - } - - generateExtendedDrivableArea(path); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); -#ifdef USE_OLD_ARCHITECTURE - path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = path; -#else - const auto reference_path = - utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_); - output.reference_path = std::make_shared(reference_path); - path_reference_ = std::make_shared(reference_path); - prev_approved_path_ = *getPreviousModuleOutput().path; -#endif - updateOutputTurnSignal(output); - - updateSteeringFactorPtr(output); - clearWaitingApproval(); - - return output; -} - -void LaneChangeModule::resetPathIfAbort() -{ - if (!is_abort_approval_requested_) { -#ifdef USE_OLD_ARCHITECTURE - const auto lateral_shift = utils::lane_change::getLateralShift(*abort_path_); - if (lateral_shift > 0.0) { - removePreviousRTCStatusRight(); - uuid_map_.at("right") = generateUUID(); - } else if (lateral_shift < 0.0) { - removePreviousRTCStatusLeft(); - uuid_map_.at("left") = generateUUID(); - } -#else - removeRTCStatus(); -#endif - RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - is_abort_approval_requested_ = true; - is_abort_path_approved_ = false; - return; - } - - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); - is_abort_path_approved_ = true; - clearWaitingApproval(); - } else { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); - is_abort_path_approved_ = false; - waitApproval(); - } -} - -CandidateOutput LaneChangeModule::planCandidate() const -{ - CandidateOutput output; - - LaneChangePath selected_path; - // Get lane change lanes -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); - const auto lane_change_lanes = utils::lane_change::getLaneChangeLanes( - planner_data_, current_lanes, lane_change_lane_length_, parameters_->prepare_duration, - direction_, type_); -#endif - - if (lane_change_lanes.empty()) { - return output; - } - -#ifdef USE_OLD_ARCHITECTURE - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); -#else - selected_path = status_.lane_change_path; -#endif - - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - - if (isAbortState()) { - selected_path = *abort_path_; - } - - if (selected_path.path.points.empty()) { - return output; - } - - output.path_candidate = selected_path.path; - output.lateral_shift = utils::lane_change::getLateralShift(selected_path); - output.start_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - - updateSteeringFactorPtr(output, selected_path); - return output; -} - -BehaviorModuleOutput LaneChangeModule::planWaitingApproval() -{ -#ifdef USE_OLD_ARCHITECTURE - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (is_within_current_lane) { - prev_approved_path_ = getReferencePath(); - } -#else - prev_approved_path_ = *getPreviousModuleOutput().path; -#endif - BehaviorModuleOutput out; - out.path = std::make_shared(prev_approved_path_); - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - -#ifndef USE_OLD_ARCHITECTURE - updateLaneChangeStatus(); -#endif - - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; -#ifdef USE_OLD_ARCHITECTURE - updateRTCStatus(candidate); - waitApproval(); -#else - updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); -#endif - is_abort_path_approved_ = false; - - return out; -} - -void LaneChangeModule::updateLaneChangeStatus() -{ -#ifdef USE_OLD_ARCHITECTURE - status_.current_lanes = utils::getCurrentLanes(planner_data_); - status_.lane_change_lanes = getLaneChangeLanes(status_.current_lanes, lane_change_lane_length_); -#else - status_.current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); - status_.lane_change_lanes = utils::lane_change::getLaneChangeLanes( - planner_data_, status_.current_lanes, lane_change_lane_length_, parameters_->prepare_duration, - direction_, type_); -#endif - - // Find lane change path - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(status_.lane_change_lanes, check_distance_, selected_path); - - // Update status - status_.is_safe = found_safe_path; - status_.lane_change_path = selected_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.lane_change_lanes); - - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, getEgoPose()); - status_.start_distance = arclength_start.length; - status_.lane_change_path.path.header = getRouteHeader(); -} - -PathWithLaneId LaneChangeModule::getReferencePath() const -{ - PathWithLaneId reference_path; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto & common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = getRouteHeader(); - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - if (current_lanes.empty()) { - return reference_path; - } - - if (reference_path.points.empty()) { - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); - } - - const int num_lane_change = - std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, 0.0); - - const double lane_change_buffer = - utils::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); - - reference_path = utils::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_->prepare_duration, - lane_change_buffer); - - const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); - - return reference_path; -} - -#ifdef USE_OLD_ARCHITECTURE -lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const -{ - lanelet::ConstLanelets lane_change_lanes; - const auto & route_handler = planner_data_->route_handler; - const auto minimum_lane_changing_length = planner_data_->parameters.minimum_lane_changing_length; - const auto prepare_duration = parameters_->prepare_duration; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - - if (current_lanes.empty()) { - return lane_change_lanes; - } - - // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); - const double lane_change_prepare_length = - std::max(current_twist.linear.x * prepare_duration, minimum_lane_changing_length); - lanelet::ConstLanelets current_check_lanes = - route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); - const auto lane_change_lane = route_handler->getLaneChangeTarget(current_check_lanes); - if (lane_change_lane) { - lane_change_lanes = route_handler->getLaneletSequence( - lane_change_lane.get(), current_pose, lane_change_lane_length, lane_change_lane_length); - } else { - lane_change_lanes.clear(); - } - - return lane_change_lanes; -} -#endif - -std::pair LaneChangeModule::getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & common_parameters = planner_data_->parameters; - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - if (lane_change_lanes.empty()) { - return std::make_pair(false, false); - } - - // find candidate paths - LaneChangePaths valid_paths; -#ifdef USE_OLD_ARCHITECTURE - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths, - &object_debug_); -#else - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *getPreviousModuleOutput().path, *route_handler, current_lanes, lane_change_lanes, current_pose, - current_twist, planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, - direction_, &valid_paths, &object_debug_); -#endif - debug_valid_path_ = valid_paths; - - if (parameters_->publish_debug_marker) { - setObjectDebugVisualization(); - } else { - debug_marker_.markers.clear(); - } - - if (valid_paths.empty()) { - return {false, false}; - } - - if (found_safe_path) { - safe_path = valid_paths.back(); - } else { - // force candidate - safe_path = valid_paths.front(); - } - - return {true, found_safe_path}; -} - -bool LaneChangeModule::isSafe() const -{ - return status_.is_safe; -} - -bool LaneChangeModule::isValidPath() const -{ - return status_.is_valid_path; -} - -bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const -{ - const auto & route_handler = planner_data_->route_handler; - - // check lane departure - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.lane_change_lanes)); - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); - const auto lanelets = utils::transformToLanelets(expanded_lanes); - - // check path points are in any lanelets - for (const auto & point : path.points) { - bool is_in_lanelet = false; - for (const auto & lanelet : lanelets) { - if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { - is_in_lanelet = true; - break; - } - } - if (!is_in_lanelet) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; - } - } - - // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); - return false; - } - - return true; -} - -bool LaneChangeModule::isNearEndOfLane() const -{ - const auto & current_pose = getEgoPose(); - const double threshold = utils::calcTotalLaneChangeLength(planner_data_->parameters); - - return (std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) - - threshold) < planner_data_->parameters.backward_length_buffer_for_end_of_lane; -} - -bool LaneChangeModule::isCurrentVelocityLow() const -{ - constexpr double threshold_ms = 10.0 * 1000 / 3600; - return getEgoTwist().linear.x < threshold_ms; -} - -bool LaneChangeModule::isAbortConditionSatisfied() -{ - is_abort_condition_satisfied_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - - if (!parameters_->enable_cancel_lane_change) { - return false; - } - - if (!is_activated_) { - return false; - } - - Pose ego_pose_before_collision; - const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - - if (!is_path_safe) { - const auto & common_parameters = planner_data_->parameters; - const bool is_within_original_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), common_parameters); - - if (is_within_original_lane) { - current_lane_change_state_ = LaneChangeStates::Cancel; - return true; - } - - // check abort enable flag - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); - - if (!parameters_->enable_abort_lane_change) { - current_lane_change_state_ = LaneChangeStates::Stop; - return false; - } - - const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, - *parameters_); - - if (!found_abort_path && !is_abort_path_approved_) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - - current_lane_change_state_ = LaneChangeStates::Abort; - - if (!is_abort_path_approved_) { - abort_path_ = std::make_shared(*found_abort_path); - } - - return true; - } - - return false; -} - -bool LaneChangeModule::isAbortState() const -{ - if (!parameters_->enable_abort_lane_change) { - return false; - } - - if (current_lane_change_state_ != LaneChangeStates::Abort) { - return false; - } - - if (!abort_path_) { - return false; - } - - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); - return true; -} - -bool LaneChangeModule::hasFinishedLaneChange() const -{ - const auto & current_pose = getEgoPose(); - const auto & lane_change_path = status_.lane_change_path.path; - const auto & lane_change_end = status_.lane_change_path.shift_line.end; - const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( - lane_change_path.points, current_pose.position, lane_change_end.position); - return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0; -} - -void LaneChangeModule::setObjectDebugVisualization() const -{ - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; - - debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showObjectInfo(object_debug_, "object_debug_info")); - add(showLerpedPose(object_debug_, "lerp_pose_before_true")); - add(showPolygonPose(object_debug_, "expected_pose")); - add(showPolygon(object_debug_, "lerped_polygon")); - add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths")); -} - -std::shared_ptr LaneChangeModule::get_debug_msg_array() const -{ - LaneChangeDebugMsgArray debug_msg_array; - debug_msg_array.lane_change_info.reserve(object_debug_.size()); - for (const auto & [uuid, debug_data] : object_debug_) { - LaneChangeDebugMsg debug_msg; - debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; - debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; - debug_msg_array.lane_change_info.push_back(debug_msg); - } - lane_change_debug_msg_array_ = debug_msg_array; - - lane_change_debug_msg_array_.header.stamp = clock_->now(); - return std::make_shared(lane_change_debug_msg_array_); -} - -void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) -{ - const auto current_pose = getEgoPose(); - const double start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); - -#ifdef USE_OLD_ARCHITECTURE - const auto turn_signal_info = output.turn_signal_info; - const uint16_t steering_factor_direction = - std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - waitApprovalLeft(start_distance, finish_distance); - return SteeringFactor::LEFT; - } - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - waitApprovalRight(start_distance, finish_distance); - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); -#else - const auto steering_factor_direction = std::invoke([&]() { - if (direction_ == Direction::LEFT) { - return SteeringFactor::LEFT; - } - if (direction_ == Direction::RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); -#endif - - // TODO(tkhmy) add handle status TRYING - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.lane_change_path.shift_line.start, status_.lane_change_path.shift_line.end}, - {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, - SteeringFactor::TURNING, ""); -} - -void LaneChangeModule::updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const -{ - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - - steering_factor_interface_ptr_->updateSteeringFactor( - {selected_path.shift_line.start, selected_path.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); -} -std_msgs::msg::Header LaneChangeModule::getRouteHeader() const -{ - return planner_data_->route_handler->getRouteHeader(); -} -void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) -{ - const auto & common_parameters = planner_data_->parameters; - const auto & route_handler = planner_data_->route_handler; - auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, status_.current_lanes, status_.lane_change_lanes); -#ifndef USE_OLD_ARCHITECTURE - drivable_lanes = utils::lane_change::combineDrivableLanes( - getPreviousModuleOutput().drivable_lanes, drivable_lanes); -#endif - const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); -} - -bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const -{ - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = *parameters_; - const auto & route_handler = planner_data_->route_handler; - const auto & path = status_.lane_change_path; - - // get lanes used for detection - const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - *route_handler, path.target_lanelets.front(), current_pose, check_distance_); - - std::unordered_map debug_data; - const auto lateral_buffer = - utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); - const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, - lane_change_parameters, lateral_buffer); - - return utils::lane_change::isLaneChangePathSafe( - path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, - status_.lane_change_path.acceleration); -} - -void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) -{ - calcTurnSignalInfo(); - const auto turn_signal_info = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - output.turn_signal_info.turn_signal.command = turn_signal_info.first.command; - - utils::lane_change::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); -} - -void LaneChangeModule::calcTurnSignalInfo() -{ - const auto get_blinker_pose = [this](const PathWithLaneId & path, const double length) { - double accumulated_length = 0.0; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > length) { - return path.points.at(i).point.pose; - } - } - - RCLCPP_WARN(getLogger(), "unable to determine blinker pose..."); - return path.points.front().point.pose; - }; - - const auto & path = status_.lane_change_path; - TurnSignalInfo turn_signal_info{}; - - turn_signal_info.desired_start_point = std::invoke([&]() { - const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto prepare_duration = parameters_->prepare_duration; - const auto diff_time = prepare_duration - blinker_start_duration; - if (diff_time < 1e-5) { - return path.path.points.front().point.pose; - } - - const auto current_twist = getEgoTwist(); - const auto diff_length = std::abs(current_twist.linear.x) * diff_time; - return get_blinker_pose(path.path, diff_length); - }); - turn_signal_info.desired_end_point = path.shift_line.end; - - turn_signal_info.required_start_point = path.shift_line.start; - const auto mid_lane_change_length = path.length.lane_changing / 2; - const auto & shifted_path = path.shifted_path.path; - turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length); - - status_.lane_change_path.turn_signal_info = turn_signal_info; -} - -void LaneChangeModule::resetParameters() -{ - is_abort_path_approved_ = false; - is_abort_approval_requested_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - abort_path_ = nullptr; - - object_debug_.clear(); - debug_marker_.markers.clear(); - resetPathCandidate(); -} - -void LaneChangeModule::acceptVisitor(const std::shared_ptr & visitor) const -{ - if (visitor) { - visitor->visitLaneChangeModule(this); - } -} - -void SceneModuleVisitor::visitLaneChangeModule(const LaneChangeModule * module) const -{ - lane_change_visitor_ = module->get_debug_msg_array(); -} -} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index bd1f4e93cfda8..69260c97fcc02 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -24,6 +24,7 @@ namespace behavior_path_planner { using route_handler::Direction; +using utils::convertToSnakeCase; LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, std::shared_ptr parameters, const Direction direction, @@ -40,7 +41,7 @@ std::shared_ptr LaneChangeModuleManager::createNewSceneMod if (type_ == LaneChangeModuleType::NORMAL) { return std::make_shared( name_, *node_, parameters_, rtc_interface_ptr_map_, - std::make_unique(parameters_, direction_)); + std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } return std::make_shared( name_, *node_, parameters_, rtc_interface_ptr_map_, @@ -62,4 +63,33 @@ void LaneChangeModuleManager::updateModuleParams( }); } +AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters) +: LaneChangeModuleManager( + node, name, config, std::move(parameters), Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) +{ + rtc_interface_ptr_map_.clear(); + const std::vector rtc_types = {"left", "right"}; + for (const auto & rtc_type : rtc_types) { + const auto snake_case_name = convertToSnakeCase(name); + const std::string rtc_interface_name = snake_case_name + "_" + rtc_type; + rtc_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name)); + } +} + +std::shared_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_shared( + name_, *node_, parameters_, avoidance_parameters_, avoidance_by_lane_change_parameters_, + rtc_interface_ptr_map_); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 880f6325926d2..2c51640be6aa6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/utils.hpp" +#include #include #include @@ -30,22 +32,19 @@ namespace behavior_path_planner { NormalLaneChange::NormalLaneChange( - const std::shared_ptr & parameters, Direction direction) -: LaneChangeBase(parameters, direction) + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction) +: LaneChangeBase(parameters, type, direction) { } -void NormalLaneChange::updateLaneChangeStatus( - const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & previous_module_path) +void NormalLaneChange::updateLaneChangeStatus() { - status_.current_lanes = utils::getCurrentLanesFromPath(prev_module_reference_path, planner_data_); - status_.lane_change_lanes = getLaneChangeLanes(status_.current_lanes); - - // Find lane change path - const auto [found_valid_path, found_safe_path] = - getSafePath(prev_module_reference_path, previous_module_path, status_.lane_change_path); + const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status + status_.current_lanes = status_.lane_change_path.reference_lanelets; + status_.lane_change_lanes = status_.lane_change_path.target_lanelets; status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); @@ -57,35 +56,24 @@ void NormalLaneChange::updateLaneChangeStatus( status_.lane_change_path.path.header = getRouteHeader(); } -std::pair NormalLaneChange::getSafePath( - const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path, - LaneChangePath & safe_path) const +std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const { - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_odometry->pose.pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto & common_parameters = planner_data_->parameters; - - const auto current_lanes = - utils::getCurrentLanesFromPath(prev_module_reference_path, planner_data_); + const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { - return std::make_pair(false, false); + return {false, false}; } - const auto lane_change_lanes = getLaneChangeLanes(current_lanes); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - if (lane_change_lanes.empty()) { - return std::make_pair(false, false); + if (target_lanes.empty()) { + return {false, false}; } // find candidate paths - LaneChangePaths valid_paths; - - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - prev_module_path, *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - planner_data_->dynamic_object, common_parameters, *parameters_, check_distance_, direction_, - &valid_paths, &object_debug_); + LaneChangePaths valid_paths{}; + const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); if (valid_paths.empty()) { return {false, false}; @@ -101,108 +89,69 @@ std::pair NormalLaneChange::getSafePath( return {true, found_safe_path}; } -PathWithLaneId NormalLaneChange::generatePlannedPath( - const std::vector & prev_drivable_lanes) +LaneChangePath NormalLaneChange::getLaneChangePath() const +{ + return isAbortState() ? *abort_path_ : status_.lane_change_path; +} + +BehaviorModuleOutput NormalLaneChange::generateOutput() { - auto path = getLaneChangePath().path; - generateExtendedDrivableArea(prev_drivable_lanes, path); + BehaviorModuleOutput output; + output.path = std::make_shared(getLaneChangePath().path); + extendOutputDrivableArea(output); + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); if (isAbortState()) { - return path; + return output; } if (isStopState()) { - const auto stop_point = utils::insertStopPoint(0.1, path); + const auto current_velocity = getEgoVelocity(); + const auto current_dist = motion_utils::calcSignedArcLength( + output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + const auto stop_dist = + -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); } - return path; -} + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); -void NormalLaneChange::generateExtendedDrivableArea( - const std::vector & prev_drivable_lanes, PathWithLaneId & path) -{ - const auto & common_parameters = planner_data_->parameters; - const auto & route_handler = planner_data_->route_handler; - auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, status_.current_lanes, status_.lane_change_lanes); - drivable_lanes = utils::lane_change::combineDrivableLanes(prev_drivable_lanes, drivable_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); -} -bool NormalLaneChange::hasFinishedLaneChange() const -{ - const auto & current_pose = getEgoPose(); - const auto & lane_change_path = status_.lane_change_path.path; - const auto & lane_change_end = status_.lane_change_path.shift_line.end; - const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( - lane_change_path.points, current_pose.position, lane_change_end.position); - return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0; -} + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); -PathWithLaneId NormalLaneChange::getReferencePath() const -{ - return utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_); + return output; } -bool NormalLaneChange::isCancelConditionSatisfied() +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) { - current_lane_change_state_ = LaneChangeStates::Normal; - - if (!parameters_->enable_cancel_lane_change) { - return false; - } - - Pose ego_pose_before_collision; - const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - - if (!is_path_safe) { - const auto & common_parameters = planner_data_->parameters; - const bool is_within_original_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), common_parameters); - - if (isNearEndOfLane() && isCurrentSpeedLow()) { - current_lane_change_state_ = LaneChangeStates::Stop; - return false; - } - - if (is_within_original_lane) { - current_lane_change_state_ = LaneChangeStates::Cancel; - return true; - } + const auto & common_parameters = planner_data_->parameters; + const auto & dp = planner_data_->drivable_area_expansion_parameters; - if (!parameters_->enable_abort_lane_change) { - return false; - } + const auto drivable_lanes = getDrivableLanes(); + const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes); + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); - return isAbortConditionSatisfied(ego_pose_before_collision); - } + // for new architecture + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + output.drivable_area_info = + utils::combineDrivableAreaInfo(current_drivable_area_info, prev_drivable_area_info_); - return false; + // for old architecture + utils::generateDrivableArea( + *output.path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_); } -bool NormalLaneChange::isAbortConditionSatisfied(const Pose & pose) +PathWithLaneId NormalLaneChange::getReferencePath() const { - const auto & common_parameters = planner_data_->parameters; - - const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, pose, common_parameters, *parameters_); - - if (!found_abort_path && !is_abort_path_approved_) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - - current_lane_change_state_ = LaneChangeStates::Abort; - - if (!is_abort_path_approved_) { - abort_path_ = std::make_shared(*found_abort_path); - } - - return true; + return utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_); } void NormalLaneChange::resetParameters() @@ -236,62 +185,480 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() return turn_signal_info; } +lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const +{ + return utils::getCurrentLanesFromPath(prev_module_reference_path_, planner_data_); +} + lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const + const lanelet::ConstLanelets & current_lanes, Direction direction) const { if (current_lanes.empty()) { return {}; } // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, getEgoPose(), ¤t_lane); + const auto & route_handler = getRouteHandler(); - const auto minimum_lane_changing_length = planner_data_->parameters.minimum_lane_changing_length; + const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( + *getRouteHandler(), current_lanes, type_, direction); - const auto lane_change_prepare_length = - std::max(getEgoVelocity() * parameters_->prepare_duration, minimum_lane_changing_length); + if (!lane_change_lane) { + return {}; + } + + const auto front_pose = std::invoke([&lane_change_lane]() { + const auto & p = lane_change_lane->centerline().front(); + const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); + const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); + geometry_msgs::msg::Pose front_pose; + front_pose.position = front_point; + tf2::Quaternion quat; + quat.setRPY(0, 0, front_yaw); + front_pose.orientation = tf2::toMsg(quat); + return front_pose; + }); + + const auto forward_length = std::invoke([&]() { + const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); + const auto forward_path_length = planner_data_->parameters.forward_path_length; + if (signed_distance <= 0.0) { + return forward_path_length; + } + + return signed_distance + forward_path_length; + }); + const auto backward_length = lane_change_parameters_->backward_lane_length; + return route_handler->getLaneletSequence( + lane_change_lane.get(), getEgoPose(), backward_length, forward_length); +} + +bool NormalLaneChange::isNearEndOfLane() const +{ const auto & route_handler = getRouteHandler(); + const auto & current_pose = getEgoPose(); + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(status_.current_lanes.back()); + const auto threshold = + utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals); - const auto current_check_lanes = - route_handler->getLaneletSequence(current_lane, getEgoPose(), 0.0, lane_change_prepare_length); + auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, status_.current_lanes); - const auto lane_change_lane = route_handler->getLaneChangeTarget(current_check_lanes, direction_); + if (route_handler->isInGoalRouteSection(status_.lane_change_lanes.back())) { + distance_to_end = std::min( + distance_to_end, + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), status_.current_lanes)); + } - if (lane_change_lane) { - return route_handler->getLaneletSequence( - lane_change_lane.get(), getEgoPose(), lane_change_lane_length_, lane_change_lane_length_); + return (std::max(0.0, distance_to_end) - threshold) < + planner_data_->parameters.backward_length_buffer_for_end_of_lane; +} + +bool NormalLaneChange::hasFinishedLaneChange() const +{ + const auto & current_pose = getEgoPose(); + const auto & lane_change_path = status_.lane_change_path.path; + const auto & lane_change_end = status_.lane_change_path.shift_line.end; + const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( + lane_change_path.points, current_pose.position, lane_change_end.position); + if (dist_to_lane_change_end + lane_change_parameters_->lane_change_finish_judge_buffer < 0.0) { + return true; } + return false; +} - return {}; +bool NormalLaneChange::isAbleToReturnCurrentLane() const +{ + if (status_.lane_change_path.path.points.size() < 2) { + return false; + } + + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + status_.lane_change_path.path.points, getEgoPose(), + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + const double ego_velocity = + std::max(getEgoVelocity(), planner_data_->parameters.minimum_lane_changing_velocity); + const double estimated_travel_dist = ego_velocity * lane_change_parameters_->abort_delta_time; + + double dist = 0.0; + for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { + dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + if (dist > estimated_travel_dist) { + const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; + return utils::lane_change::isEgoWithinOriginalLane( + status_.current_lanes, estimated_pose, planner_data_->parameters); + } + } + + return true; } -bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) const +bool NormalLaneChange::isEgoOnPreparePhase() const +{ + const auto & start_position = status_.lane_change_path.shift_line.start.position; + const auto & path_points = status_.lane_change_path.path.points; + return motion_utils::calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; +} + +bool NormalLaneChange::isAbleToStopSafely() const +{ + if (status_.lane_change_path.path.points.size() < 2) { + return false; + } + + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + status_.lane_change_path.path.points, getEgoPose(), + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + const auto current_velocity = getEgoVelocity(); + const auto stop_dist = + -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + + double dist = 0.0; + for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { + dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + if (dist > stop_dist) { + const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; + return utils::lane_change::isEgoWithinOriginalLane( + status_.current_lanes, estimated_pose, planner_data_->parameters); + } + } + return true; +} + +bool NormalLaneChange::hasFinishedAbort() const +{ + if (!abort_path_) { + return true; + } + + const auto distance_to_finish = motion_utils::calcSignedArcLength( + abort_path_->path.points, getEgoPosition(), abort_path_->shift_line.end.position); + + if (distance_to_finish < 0.0) { + return true; + } + + return false; +} + +bool NormalLaneChange::isAbortState() const +{ + if (!lane_change_parameters_->enable_abort_lane_change) { + return false; + } + + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; + } + + if (!abort_path_) { + return false; + } + + return true; +} +int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const +{ + const auto get_opposite_direction = + (direction_ == Direction::RIGHT) ? Direction::LEFT : Direction::RIGHT; + return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); +} + +PathWithLaneId NormalLaneChange::getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, + [[maybe_unused]] const double arc_length_from_current, const double backward_path_length, + const double prepare_length, const double current_velocity, const double prepare_velocity) const +{ + if (current_lanes.empty()) { + return PathWithLaneId(); + } + + auto prepare_segment = prev_module_path_; + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, getEgoPose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); + + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); + + return prepare_segment; +} + +bool NormalLaneChange::getLaneChangePaths( + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + Direction direction, LaneChangePaths * candidate_paths) const +{ + object_debug_.clear(); + if (original_lanelets.empty() || target_lanelets.empty()) { + return false; + } + const auto & route_handler = *getRouteHandler(); + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & common_parameter = planner_data_->parameters; + + const auto backward_path_length = common_parameter.backward_path_length; + const auto forward_path_length = common_parameter.forward_path_length; + const auto prepare_duration = common_parameter.lane_change_prepare_duration; + const auto minimum_prepare_length = common_parameter.minimum_prepare_length; + const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; + const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; + const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; + + // get velocity + const auto current_velocity = getEgoTwist().linear.x; + + // compute maximum longitudinal deceleration and acceleration + const auto maximum_deceleration = + std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, this]() { + const double min_a = (minimum_lane_changing_velocity - current_velocity) / + common_parameter.lane_change_prepare_duration; + return std::clamp( + min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); + }); + const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration( + prev_module_path_, getEgoPose(), current_velocity, common_parameter); + + // get sampling acceleration values + const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues( + maximum_deceleration, maximum_acceleration, longitudinal_acc_sampling_num); + + const auto target_length = + utils::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), getEgoPose()); + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); + + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back()); + const double next_lane_change_buffer = + utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + + const auto dist_to_end_of_current_lanes = + utils::getDistanceToEndOfLane(getEgoPose(), original_lanelets); + + [[maybe_unused]] const auto arc_position_from_current = + lanelet::utils::getArcCoordinates(original_lanelets, getEgoPose()); + const auto arc_position_from_target = + lanelet::utils::getArcCoordinates(target_lanelets, getEgoPose()); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); + + const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds( + route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); + const auto lateral_buffer = + utils::lane_change::calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); + + LaneChangeTargetObjectIndices dynamic_object_indices; + + candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); + for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + const auto prepare_velocity = std::max( + current_velocity + sampled_longitudinal_acc * prepare_duration, + minimum_lane_changing_velocity); + + // compute actual longitudinal acceleration + const double longitudinal_acc = (prepare_velocity - current_velocity) / prepare_duration; + + // get path on original lanes + const double prepare_length = std::max( + current_velocity * prepare_duration + 0.5 * longitudinal_acc * std::pow(prepare_duration, 2), + minimum_prepare_length); + + if (prepare_length < target_length) { + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "prepare length is shorter than distance to target lane!!"); + break; + } + + const auto prepare_segment = getPrepareSegment( + original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length, + current_velocity, prepare_velocity); + + if (prepare_segment.points.empty()) { + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "prepare segment is empty!!"); + continue; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + original_lanelets, target_lanelets.front(), lane_changing_start_pose); + // In new architecture, there is a possibility that the lane change start getEgoPose() is behind + // of the target lanelet, even if the condition prepare_length > target_length is satisfied. In + // that case, the lane change shouldn't be executed. + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + break; + } + + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); + + // we assume constant velocity during lane change + const auto lane_changing_velocity = prepare_velocity; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_parameter.lane_change_lat_acc_map.find(lane_changing_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; + constexpr double lateral_acc_epsilon = 0.01; + + for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; + lateral_acc += lateral_acc_resolution) { + const auto lane_changing_length = utils::lane_change::calcLaneChangingLength( + lane_changing_velocity, shift_length, lateral_acc, + common_parameter.lane_changing_lateral_jerk); + + if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "length of lane changing path is longer than length to goal!!"); + continue; + } + + if (is_goal_in_route) { + const double s_start = + lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length; + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; + if ( + s_start + lane_changing_length + + lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer > + s_goal) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), + "length of lane changing path is longer than length to goal!!"); + continue; + } + } + + const auto target_segment = utils::lane_change::getTargetSegment( + route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, + target_lane_length, lane_changing_length, lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), + "target segment is empty!! something wrong..."); + continue; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_changing_length, lane_changing_velocity); + + const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, + lc_length.lane_changing, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), + "target_lane_reference_path is empty!!"); + continue; + } + + const auto shift_line = utils::lane_change::getLaneChangingShiftLine( + prepare_segment, target_segment, target_lane_reference_path, shift_length); + + const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity}; + + const auto candidate_path = utils::lane_change::constructCandidatePath( + prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, + target_lanelets, sorted_lane_ids, longitudinal_acc, lateral_acc, lc_length, lc_velocity, + common_parameter, *lane_change_parameters_); + + if (!candidate_path) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), + "no candidate path!!"); + continue; + } + + const auto is_valid = utils::lane_change::hasEnoughLength( + *candidate_path, original_lanelets, target_lanelets, getEgoPose(), route_handler, + minimum_lane_changing_velocity, common_parameter, direction); + + if (!is_valid) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), + "invalid candidate path!!"); + continue; + } + + if (candidate_paths->empty()) { + // only compute dynamic object indices once + const auto backward_length = lane_change_parameters_->backward_lane_length; + const auto backward_target_lanes_for_object_filtering = + utils::lane_change::getBackwardLanelets( + route_handler, target_lanelets, getEgoPose(), backward_length); + dynamic_object_indices = utils::lane_change::filterObjectIndices( + {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, + getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, + lateral_buffer); + } + candidate_paths->push_back(*candidate_path); + + const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe( + *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), + common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, + common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc); + + if (is_safe) { + return true; + } + } + } + + return false; +} + +std::vector NormalLaneChange::getDrivableLanes() const +{ + const auto drivable_lanes = utils::lane_change::generateDrivableLanes( + *getRouteHandler(), status_.current_lanes, status_.lane_change_lanes); + return utils::combineDrivableLanes(prev_drivable_area_info_.drivable_lanes, drivable_lanes); +} + +PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto current_pose = getEgoPose(); const auto current_twist = getEgoTwist(); const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = *parameters_; - const auto & route_handler = planner_data_->route_handler; + const auto & common_parameters = getCommonParam(); + const auto & lane_change_parameters = *lane_change_parameters_; + const auto & route_handler = getRouteHandler(); const auto & path = status_.lane_change_path; // get lanes used for detection - const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - *route_handler, path.target_lanelets.front(), current_pose, check_distance_); + const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( + *route_handler, path.target_lanelets, current_pose, + lane_change_parameters.backward_lane_length); CollisionCheckDebugMap debug_data; const auto lateral_buffer = utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, - lane_change_parameters, lateral_buffer); + {path}, *dynamic_objects, backward_target_lanes_for_object_filtering, current_pose, + common_parameters.forward_path_length, lane_change_parameters, lateral_buffer); - return utils::lane_change::isLaneChangePathSafe( + const auto safety_status = utils::lane_change::isLaneChangePathSafe( path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, + *lane_change_parameters_, common_parameters.expected_front_deceleration_for_abort, + common_parameters.expected_rear_deceleration_for_abort, debug_data, status_.lane_change_path.acceleration); + + return safety_status; } void NormalLaneChange::calcTurnSignalInfo() @@ -314,7 +681,7 @@ void NormalLaneChange::calcTurnSignalInfo() turn_signal_info.desired_start_point = std::invoke([&]() { const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto prepare_duration = parameters_->prepare_duration; + const auto prepare_duration = planner_data_->parameters.lane_change_prepare_duration; const auto diff_time = prepare_duration - blinker_start_duration; if (diff_time < 1e-5) { return path.path.points.front().point.pose; @@ -337,14 +704,16 @@ void NormalLaneChange::calcTurnSignalInfo() bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler; + const auto & dp = planner_data_->drivable_area_expansion_parameters; // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *route_handler, utils::extendLanes(route_handler, status_.current_lanes), utils::extendLanes(route_handler, status_.lane_change_lanes)); const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + const auto lanelets = utils::transformToLanelets(expanded_lanes); // check path points are in any lanelets @@ -368,4 +737,254 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const return true; } + +bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const +{ + return isNearEndOfLane() && isAbleToStopSafely() && is_object_coming_from_rear; +} + +bool NormalLaneChange::getAbortPath() +{ + const auto & route_handler = getRouteHandler(); + const auto & common_param = getCommonParam(); + const auto current_velocity = + std::max(common_param.minimum_lane_changing_velocity, getEgoVelocity()); + const auto current_pose = getEgoPose(); + const auto & selected_path = status_.lane_change_path; + const auto reference_lanelets = selected_path.reference_lanelets; + + const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + + const auto direction = getDirection(); + const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( + selected_path.reference_lanelets.back(), direction); + const double minimum_lane_change_length = + utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + + const auto & lane_changing_path = selected_path.path; + const auto lane_changing_end_pose_idx = std::invoke([&]() { + constexpr double s_start = 0.0; + const double s_end = std::max( + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); + + const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + }); + + const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + + const auto get_abort_idx_and_distance = [&](const double param_time) { + if (ego_pose_idx > lane_changing_end_pose_idx) { + return std::make_pair(ego_pose_idx, 0.0); + } + + const auto desired_distance = current_velocity * param_time; + const auto & points = lane_changing_path.points; + + for (size_t idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + const double distance = + utils::getSignedDistance(current_pose, points.at(idx).point.pose, reference_lanelets); + if (distance > desired_distance) { + return std::make_pair(idx, distance); + } + } + + return std::make_pair(ego_pose_idx, 0.0); + }; + + const auto [abort_start_idx, abort_start_dist] = + get_abort_idx_and_distance(lane_change_parameters_->abort_delta_time); + const auto [abort_return_idx, abort_return_dist] = get_abort_idx_and_distance( + lane_change_parameters_->abort_delta_time + lane_change_parameters_->aborting_time); + + if (abort_start_idx >= abort_return_idx) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "abort start idx and return idx is equal. can't compute abort path."); + return false; + } + + if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( + *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param, + direction)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "insufficient distance to abort."); + return false; + } + + const auto abort_start_pose = lane_changing_path.points.at(abort_start_idx).point.pose; + const auto abort_return_pose = lane_changing_path.points.at(abort_return_idx).point.pose; + const auto shift_length = + lanelet::utils::getArcCoordinates(reference_lanelets, abort_return_pose).distance; + + ShiftLine shift_line; + shift_line.start = abort_start_pose; + shift_line.end = abort_return_pose; + shift_line.end_shift_length = -shift_length; + shift_line.start_idx = abort_start_idx; + shift_line.end_idx = abort_return_idx; + + PathShifter path_shifter; + path_shifter.setPath(lane_changing_path); + path_shifter.addShiftLine(shift_line); + const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( + shift_line.end_shift_length, abort_start_dist, current_velocity); + path_shifter.setVelocity(current_velocity); + const auto lateral_acc_range = common_param.lane_change_lat_acc_map.find(current_velocity); + const double & max_lateral_acc = lateral_acc_range.second; + path_shifter.setLateralAccelerationLimit(max_lateral_acc); + + if (lateral_jerk > lane_change_parameters_->abort_max_lateral_jerk) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "Aborting jerk is too strong. lateral_jerk = " << lateral_jerk); + return false; + } + + ShiftedPath shifted_path; + // offset front side + // bool offset_back = false; + if (!path_shifter.generate(&shifted_path)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), + "failed to generate abort shifted path."); + } + + const auto arc_position = lanelet::utils::getArcCoordinates( + reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); + const PathWithLaneId reference_lane_segment = std::invoke([&]() { + const double s_start = arc_position.length; + double s_end = std::max( + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, s_start); + + if (route_handler->isInGoalRouteSection(selected_path.target_lanelets.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); + const double forward_length = + std::max(goal_arc_coordinates.length - minimum_lane_change_length, s_start); + s_end = std::min(s_end, forward_length); + } + PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); + ref.points.back().point.longitudinal_velocity_mps = std::min( + ref.points.back().point.longitudinal_velocity_mps, + static_cast(common_param.minimum_lane_changing_velocity)); + return ref; + }); + + PathWithLaneId start_to_abort_return_pose; + start_to_abort_return_pose.points.insert( + start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + if (reference_lane_segment.points.size() > 1) { + start_to_abort_return_pose.points.insert( + start_to_abort_return_pose.points.end(), (reference_lane_segment.points.begin() + 1), + reference_lane_segment.points.end()); + } + + auto abort_path = selected_path; + abort_path.shifted_path = shifted_path; + abort_path.path = start_to_abort_return_pose; + abort_path.shift_line = shift_line; + abort_path_ = std::make_shared(abort_path); + return true; +} + +NormalLaneChangeBT::NormalLaneChangeBT( + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction) +: NormalLaneChange(parameters, type, direction) +{ +} + +PathWithLaneId NormalLaneChangeBT::getReferencePath() const +{ + PathWithLaneId reference_path; + if (!utils::lane_change::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters)) { + return reference_path; + } + + const auto & route_handler = getRouteHandler(); + const auto & current_pose = getEgoPose(); + const auto & common_parameters = planner_data_->parameters; + + // Set header + reference_path.header = getRouteHeader(); + + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return reference_path; + } + + reference_path = utils::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters); + + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const double lane_change_buffer = + utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); + + reference_path = utils::setDecelerationVelocity( + *route_handler, reference_path, current_lanes, common_parameters.lane_change_prepare_duration, + lane_change_buffer); + + const auto & dp = planner_data_->drivable_area_expansion_parameters; + + const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + utils::generateDrivableArea( + reference_path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_); + + return reference_path; +} + +lanelet::ConstLanelets NormalLaneChangeBT::getCurrentLanes() const +{ + return utils::getCurrentLanes(planner_data_); +} + +int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const +{ + return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane)); +} + +PathWithLaneId NormalLaneChangeBT::getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, const double current_velocity, + const double prepare_velocity) const +{ + if (current_lanes.empty()) { + return PathWithLaneId(); + } + + const double s_start = arc_length_from_current - backward_path_length; + const double s_end = arc_length_from_current + prepare_length; + + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()).get_child("getPrepareSegment"), + "start: %f, end: %f", s_start, s_end); + + PathWithLaneId prepare_segment = + getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); + + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); + + return prepare_segment; +} + +std::vector NormalLaneChangeBT::getDrivableLanes() const +{ + return utils::lane_change::generateDrivableLanes( + *getRouteHandler(), status_.current_lanes, status_.lane_change_lanes); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 46d5b01f294aa..2c0985251b845 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -24,11 +24,9 @@ namespace behavior_path_planner { -LaneFollowingModule::LaneFollowingModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters) +LaneFollowingModule::LaneFollowingModule(const std::string & name, rclcpp::Node & node) // RTCInterface is temporarily registered, but not used. -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters} +: SceneModuleInterface{name, node, {}} { initParam(); } @@ -50,8 +48,7 @@ bool LaneFollowingModule::isExecutionReady() const BT::NodeStatus LaneFollowingModule::updateState() { - current_state_ = BT::NodeStatus::SUCCESS; - return current_state_; + return BT::NodeStatus::SUCCESS; } BehaviorModuleOutput LaneFollowingModule::plan() @@ -77,24 +74,21 @@ void LaneFollowingModule::processOnExit() current_state_ = BT::NodeStatus::SUCCESS; } -void LaneFollowingModule::setParameters(const std::shared_ptr & parameters) -{ - parameters_ = parameters; -} - BehaviorModuleOutput LaneFollowingModule::getReferencePath() const { const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_odometry->pose.pose; + const double dist_threshold = planner_data_->parameters.ego_nearest_dist_threshold; + const double yaw_threshold = planner_data_->parameters.ego_nearest_yaw_threshold; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( - current_pose, ¤t_lane, parameters_->distance_threshold, parameters_->yaw_threshold)) { + current_pose, ¤t_lane, dist_threshold, yaw_threshold)) { RCLCPP_ERROR_THROTTLE( getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!"); return {}; // TODO(Horibe) } - return utils::getReferencePath(current_lane, parameters_, planner_data_); + return utils::getReferencePath(current_lane, planner_data_); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 18acee33cc521..a384d61fb9d63 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -51,8 +51,7 @@ PullOutModule::PullOutModule( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - pull_out_planners_.push_back( - std::make_shared(node, *parameters, getGeometricPullOutParameters())); + pull_out_planners_.push_back(std::make_shared(node, *parameters)); } if (pull_out_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -76,8 +75,7 @@ PullOutModule::PullOutModule( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - pull_out_planners_.push_back( - std::make_shared(node, *parameters, getGeometricPullOutParameters())); + pull_out_planners_.push_back(std::make_shared(node, *parameters)); } if (pull_out_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -98,34 +96,6 @@ BehaviorModuleOutput PullOutModule::run() return plan(); } -void PullOutModule::processOnEntry() -{ - // initialize when receiving new route - if ( - last_route_received_time_ == nullptr || - *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { - RCLCPP_INFO(getLogger(), "Receive new route, so reset status"); - resetStatus(); - updatePullOutStatus(); - } - last_route_received_time_ = - std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); - - // for preventing chattering between back and pull_out - if (!status_.back_finished) { - if (last_pull_out_start_update_time_ == nullptr) { - last_pull_out_start_update_time_ = std::make_unique(clock_->now()); - } - const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); - if (elapsed_time < parameters_->backward_path_update_duration) { - return; - } - last_pull_out_start_update_time_ = std::make_unique(clock_->now()); - } - - updatePullOutStatus(); -} - void PullOutModule::processOnExit() { resetPathCandidate(); @@ -183,8 +153,7 @@ ModuleStatus PullOutModule::updateState() RCLCPP_DEBUG(getLogger(), "PULL_OUT updateState"); if (hasFinishedPullOut()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; + return ModuleStatus::SUCCESS; } checkBackFinished(); @@ -209,6 +178,12 @@ BehaviorModuleOutput PullOutModule::plan() // the path of getCurrent() is generated by generateStopPath() const PathWithLaneId stop_path = getCurrentPath(); output.path = std::make_shared(stop_path); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = status_.lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -226,12 +201,14 @@ BehaviorModuleOutput PullOutModule::plan() path = status_.backward_path; } - const auto shorten_lanes = utils::cutOverlappedLanes(path, status_.lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; @@ -320,6 +297,7 @@ PathWithLaneId PullOutModule::getFullPath() const BehaviorModuleOutput PullOutModule::planWaitingApproval() { + updatePullOutStatus(); waitApproval(); BehaviorModuleOutput output; @@ -337,20 +315,26 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); + auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - - auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); utils::generateDrivableArea( - stop_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + stop_path, expanded_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); for (auto & p : stop_path.points) { p.point.longitudinal_velocity_mps = 0.0; } + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + output.path = std::make_shared(stop_path); + output.drivable_area_info.drivable_lanes = status_.lanes; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); @@ -398,20 +382,6 @@ void PullOutModule::resetStatus() status_ = initial_status; } -ParallelParkingParameters PullOutModule::getGeometricPullOutParameters() const -{ - ParallelParkingParameters params{}; - - params.th_arrived_distance = parameters_->th_arrived_distance; - params.th_stopped_velocity = parameters_->th_stopped_velocity; - params.arc_path_interval = parameters_->arc_path_interval; - params.departing_velocity = parameters_->geometric_pull_out_velocity; - params.departing_lane_departure_margin = parameters_->lane_departure_margin; - params.max_steer_angle = parameters_->pull_out_max_steer_angle; - - return params; -} - void PullOutModule::incrementPathIndex() { status_.current_path_idx = @@ -551,18 +521,40 @@ PathWithLaneId PullOutModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(moved_pose)); // generate drivable area - const auto shorten_lanes = utils::cutOverlappedLanes(path, status_.lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); + + // for old architecture utils::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); return path; } void PullOutModule::updatePullOutStatus() { + // if new route is received, reset status + const bool has_received_new_route = + last_route_received_time_ == nullptr || + *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp; + if (has_received_new_route) { + RCLCPP_INFO(getLogger(), "Receive new route, so reset status"); + resetStatus(); + } + last_route_received_time_ = + std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); + + // skip updating if enough time has not passed for preventing chattering between back and pull_out + if (!has_received_new_route && !status_.back_finished) { + if (last_pull_out_start_update_time_ == nullptr) { + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); + if (elapsed_time < parameters_->backward_path_update_duration) { + return; + } + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); @@ -697,14 +689,28 @@ bool PullOutModule::hasFinishedPullOut() const return false; } - // check ego car is close enough to goal pose const auto current_pose = planner_data_->self_odometry->pose.pose; + + // keep running until returning to the path, considering that other modules (e.g avoidance) + // are also running at the same time. + const double lateral_offset_to_path = + motion_utils::calcLateralOffset(getCurrentPath().points, current_pose.position); + constexpr double lateral_offset_threshold = 0.5; + if (std::abs(lateral_offset_to_path) > lateral_offset_threshold) { + return false; + } + const double yaw_deviation = + motion_utils::calcYawDeviation(getCurrentPath().points, current_pose); + constexpr double yaw_deviation_threshold = 0.5; + if (std::abs(yaw_deviation) > yaw_deviation_threshold) { + return false; + } + + // check that ego has passed pull out end point const auto arclength_current = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); const auto arclength_pull_out_end = lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); - - // has passed pull out end point return arclength_current.length - arclength_pull_out_end.length > 0.0; } @@ -813,7 +819,9 @@ void PullOutModule::setDebugData() const }; debug_marker_.markers.clear(); - add(createPoseMarkerArray(status_.pull_out_start_pose, "pull_out_start_pose", 0, 0.9, 0.3, 0.3)); + add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3)); + add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); + add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp index 7f90ad46feff4..855e099c1ce21 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp @@ -81,7 +81,8 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error()); } - current_status = scene_module_->updateState(); + scene_module_->updateCurrentState(); + current_status = scene_module_->getCurrentStatus(); // for data output module_status_->status = current_status; diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 2ac72eb80598e..91ae50e196b0a 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -26,7 +26,7 @@ namespace behavior_path_planner SideShiftModuleManager::SideShiftModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} +: SceneModuleManagerInterface(node, name, config, {}), parameters_{parameters} { } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index da228e16b8b6f..12515d88b1afd 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" +#include "behavior_path_planner/marker_util/debug_utilities.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/side_shift/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -38,7 +39,7 @@ using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters} +: SceneModuleInterface{name, node, {}}, parameters_{parameters} { using std::placeholders::_1; lateral_offset_subscriber_ = node.create_subscription( @@ -58,6 +59,8 @@ SideShiftModule::SideShiftModule( void SideShiftModule::initVariables() { reference_path_ = PathWithLaneId(); + debug_data_.path_shifter.reset(); + debug_marker_.markers.clear(); start_pose_reset_request_ = false; requested_lateral_offset_ = 0.0; inserted_lateral_offset_ = 0.0; @@ -159,41 +162,53 @@ ModuleStatus SideShiftModule::updateState() no_shifted_plan); if (no_request && no_shifted_plan && no_offset_diff) { - current_state_ = ModuleStatus::SUCCESS; - } else { - const auto & current_lanes = utils::getCurrentLanes(planner_data_); - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & inserted_shift_line_start_pose = inserted_shift_line_.start; - const auto & inserted_shift_line_end_pose = inserted_shift_line_.end; - const double self_to_shift_line_start_arc_length = - behavior_path_planner::utils::getSignedDistance( - current_pose, inserted_shift_line_start_pose, current_lanes); - const double self_to_shift_line_end_arc_length = - behavior_path_planner::utils::getSignedDistance( - current_pose, inserted_shift_line_end_pose, current_lanes); - if (self_to_shift_line_start_arc_length >= 0) { - shift_status_ = SideShiftStatus::BEFORE_SHIFT; - } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) { - shift_status_ = SideShiftStatus::SHIFTING; - } else { - shift_status_ = SideShiftStatus::AFTER_SHIFT; - } - current_state_ = ModuleStatus::RUNNING; + return ModuleStatus::SUCCESS; } - return current_state_; + const auto & current_lanes = utils::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & inserted_shift_line_start_pose = inserted_shift_line_.start; + const auto & inserted_shift_line_end_pose = inserted_shift_line_.end; + const double self_to_shift_line_start_arc_length = + behavior_path_planner::utils::getSignedDistance( + current_pose, inserted_shift_line_start_pose, current_lanes); + const double self_to_shift_line_end_arc_length = behavior_path_planner::utils::getSignedDistance( + current_pose, inserted_shift_line_end_pose, current_lanes); + if (self_to_shift_line_start_arc_length >= 0) { + shift_status_ = SideShiftStatus::BEFORE_SHIFT; + } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) { + shift_status_ = SideShiftStatus::SHIFTING; + } else { + shift_status_ = SideShiftStatus::AFTER_SHIFT; + } + return ModuleStatus::RUNNING; } void SideShiftModule::updateData() { - if (prev_reference_.points.empty()) { - prev_reference_ = *getPreviousModuleOutput().path; - } + // special for avoidance: take behind distance upt ot shift-start-point if it exist. + const auto longest_dist_to_shift_line = [&]() { + double max_dist = 0.0; + for (const auto & pnt : path_shifter_.getShiftLines()) { + max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); + } + return max_dist; + }(); const auto reference_pose = prev_output_.shift_length.empty() ? planner_data_->self_odometry->pose.pose - : getUnshiftedEgoPose(prev_output_); - const auto centerline_path = calcCenterLinePath(planner_data_, reference_pose); + : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); +#ifdef USE_OLD_ARCHITECTURE + const auto centerline_path = + utils::calcCenterLinePath(planner_data_, reference_pose, longest_dist_to_shift_line); +#else + if (prev_reference_.points.empty()) { + prev_reference_ = *getPreviousModuleOutput().path; + } + const auto centerline_path = utils::calcCenterLinePath( + planner_data_, reference_pose, longest_dist_to_shift_line, + *getPreviousModuleOutput().reference_path); +#endif constexpr double resample_interval = 1.0; #ifdef USE_OLD_ARCHITECTURE @@ -222,10 +237,13 @@ void SideShiftModule::updateData() path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); #ifndef USE_OLD_ARCHITECTURE - if (planner_data_->lateral_offset != nullptr) { + if ( + planner_data_->lateral_offset != nullptr && + planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) { if (isReadyForNextRequest(parameters_->shift_request_time_limit)) { lateral_offset_change_request_ = true; requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset; + latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp; } } #endif @@ -278,15 +296,20 @@ BehaviorModuleOutput SideShiftModule::plan() // Reset orientation setOrientation(&shifted_path.path); - adjustDrivableArea(&shifted_path); - - BehaviorModuleOutput output; - output.path = std::make_shared(shifted_path.path); + auto output = adjustDrivableArea(shifted_path); output.reference_path = getPreviousModuleOutput().reference_path; prev_output_ = shifted_path; path_reference_ = getPreviousModuleOutput().reference_path; + debug_data_.path_shifter = std::make_shared(path_shifter_); + + if (parameters_->publish_debug_marker) { + setDebugMarkersVisualization(); + } else { + debug_marker_.markers.clear(); + } + return output; } @@ -315,11 +338,10 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() // Reset orientation setOrientation(&shifted_path.path); - adjustDrivableArea(&shifted_path); + auto output = adjustDrivableArea(shifted_path); - BehaviorModuleOutput output; - output.path = std::make_shared(shifted_path.path); output.reference_path = getPreviousModuleOutput().reference_path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; @@ -398,47 +420,48 @@ double SideShiftModule::getClosestShiftLength() const return prev_output_.shift_length.at(closest); } -void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const +BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & path) const { - const auto itr = std::minmax_element(path->shift_length.begin(), path->shift_length.end()); + BehaviorModuleOutput out; + const auto & p = planner_data_->parameters; + + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto itr = std::minmax_element(path.shift_length.begin(), path.shift_length.end()); constexpr double threshold = 0.1; constexpr double margin = 0.5; const double left_offset = std::max( - *itr.second + (*itr.first > threshold ? margin : 0.0), - parameters_->drivable_area_left_bound_offset); + *itr.second + (*itr.first > threshold ? margin : 0.0), dp.drivable_area_left_bound_offset); const double right_offset = -std::min( - *itr.first - (*itr.first < -threshold ? margin : 0.0), - -parameters_->drivable_area_right_bound_offset); + *itr.first - (*itr.first < -threshold ? margin : 0.0), -dp.drivable_area_right_bound_offset); - const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_); - const auto shorten_lanes = utils::cutOverlappedLanes(path->path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, left_offset, right_offset, parameters_->drivable_area_types_to_skip); + // crop path which is too long here + auto output_path = path.path; + const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(output_path.points); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + output_path.points = motion_utils::cropPoints( + output_path.points, current_pose.position, current_seg_idx, p.forward_path_length, + p.backward_path_length + p.input_path_interval); - { - const auto & p = planner_data_->parameters; - utils::generateDrivableArea(path->path, expanded_lanes, p.vehicle_length, planner_data_); + const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_); + const auto shorten_lanes = utils::cutOverlappedLanes(output_path, drivable_lanes); + const auto expanded_lanes = + utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip); + + { // for old architecture + utils::generateDrivableArea( + output_path, expanded_lanes, false, p.vehicle_length, planner_data_); + out.path = std::make_shared(output_path); } -} -// NOTE: this function is ported from avoidance. -Pose SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const -{ - const auto ego_pose = getEgoPose(); - if (prev_path.path.points.empty()) { - return ego_pose; + { // for new architecture + // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be + // assigned without combining. + out.drivable_area_info.drivable_lanes = expanded_lanes; + out.drivable_area_info.is_already_expanded = true; } - // un-shifted fot current ideal pose - const auto closest = motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); - - Pose unshifted_pose = ego_pose; - - unshifted_pose = calcOffsetPose(unshifted_pose, 0.0, -prev_path.shift_length.at(closest), 0.0); - unshifted_pose.orientation = ego_pose.orientation; - - return unshifted_pose; + return out; } PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & original_path) const @@ -484,40 +507,25 @@ PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & orig return extended_path; } -// NOTE: this function is ported from avoidance. -PathWithLaneId SideShiftModule::calcCenterLinePath( - const std::shared_ptr & planner_data, const Pose & pose) const +void SideShiftModule::setDebugMarkersVisualization() const { - const auto & p = planner_data->parameters; - const auto & route_handler = planner_data->route_handler; - - PathWithLaneId centerline_path; + using marker_utils::createShiftLineMarkerArray; - // special for avoidance: take behind distance upt ot shift-start-point if it exist. - const auto longest_dist_to_shift_line = [&]() { - double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); - } - return max_dist; - }(); - const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. - const auto backward_length = - std::max(p.backward_path_length, longest_dist_to_shift_line + extra_margin); - - RCLCPP_DEBUG( - getLogger(), - "p.backward_path_length = %f, longest_dist_to_shift_line = %f, backward_length = %f", - p.backward_path_length, longest_dist_to_shift_line, backward_length); + debug_marker_.markers.clear(); - const lanelet::ConstLanelets current_lanes = - utils::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); - centerline_path = utils::getCenterLinePath( - *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; - centerline_path.header = route_handler->getRouteHeader(); + const auto add_shift_line_marker = [this, add]( + const auto & ns, auto r, auto g, auto b, double w = 0.1) { + add(createShiftLineMarkerArray( + debug_data_.path_shifter->getShiftLines(), debug_data_.path_shifter->getBaseOffset(), ns, r, + g, b, w)); + }; - return centerline_path; + if (debug_data_.path_shifter) { + add_shift_line_marker("side_shift_shift_points", 0.7, 0.7, 0.7, 0.4); + } } - } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 61723bee6743b..9234f06bf897d 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -28,35 +28,40 @@ namespace behavior_path_planner { +double calc_distance( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) +{ + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); +} + TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( - const std::shared_ptr & planner_data, const PathWithLaneId & path, - const TurnSignalInfo & turn_signal_info) + const std::shared_ptr & route_handler, const PathWithLaneId & path, + const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, + const BehaviorPathPlannerParameters & parameters) { // Guard if (path.points.empty()) { return turn_signal_info.turn_signal; } - // Data - const double nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - const auto & current_pose = planner_data->self_odometry->pose.pose; - const double & current_vel = planner_data->self_odometry->twist.twist.linear.x; - const auto route_handler = *(planner_data->route_handler); - // Get current lanelets - const double forward_length = planner_data->parameters.forward_path_length; + const double forward_length = parameters.forward_path_length; + const double nearest_dist_threshold = parameters.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = parameters.ego_nearest_yaw_threshold; const double backward_length = 50.0; - const lanelet::ConstLanelets current_lanes = utils::calcLaneAroundPose( - planner_data->route_handler, current_pose, forward_length, backward_length); + const lanelet::ConstLanelets current_lanes = + utils::calcLaneAroundPose(route_handler, current_pose, forward_length, backward_length); if (current_lanes.empty()) { return turn_signal_info.turn_signal; } const PathWithLaneId extended_path = utils::getCenterLinePath( - route_handler, current_lanes, current_pose, backward_length, forward_length, - planner_data->parameters); + *route_handler, current_lanes, current_pose, backward_length, forward_length, parameters); if (extended_path.points.empty()) { return turn_signal_info.turn_signal; @@ -68,11 +73,21 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( // Get closest intersection turn signal if exists const auto intersection_turn_signal_info = getIntersectionTurnSignalInfo( - extended_path, current_pose, current_vel, ego_seg_idx, route_handler, nearest_dist_threshold, + extended_path, current_pose, current_vel, ego_seg_idx, *route_handler, nearest_dist_threshold, nearest_yaw_threshold); if (!intersection_turn_signal_info) { initialize_intersection_info(); + const auto & desired_end_point = turn_signal_info.desired_end_point; + const double dist_to_end_point = calc_distance( + extended_path, current_pose, ego_seg_idx, desired_end_point, nearest_dist_threshold, + nearest_yaw_threshold); + if (dist_to_end_point < 0.0) { + TurnIndicatorsCommand updated_turn_signal; + updated_turn_signal.stamp = turn_signal_info.turn_signal.stamp; + updated_turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + return updated_turn_signal; + } return turn_signal_info.turn_signal; } else if ( turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND || @@ -224,10 +239,9 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( const double nearest_dist_threshold, const double nearest_yaw_threshold) { const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( - path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); + return calc_distance( + path, current_pose, current_seg_idx, input_point, nearest_dist_threshold, + nearest_yaw_threshold); }; const auto & inter_desired_start_point = intersection_signal_info.desired_start_point; @@ -314,6 +328,79 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) +{ + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); + }; + + const auto & original_desired_start_point = original_signal.desired_start_point; + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & original_required_start_point = original_signal.required_start_point; + const auto & original_required_end_point = original_signal.required_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + const auto & new_desired_end_point = new_signal.desired_end_point; + const auto & new_required_start_point = new_signal.required_start_point; + const auto & new_required_end_point = new_signal.required_end_point; + + const double dist_to_original_desired_start = + get_distance(original_desired_start_point) - base_link2front_; + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + + // If we still do not reach the desired front point we ignore it + if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { + TurnSignalInfo empty_signal_info; + return empty_signal_info; + } else if (dist_to_original_desired_start > 0.0) { + return new_signal; + } else if (dist_to_new_desired_start > 0.0) { + return original_signal; + } + + // If we already passed the desired end point, return the other signal + if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { + TurnSignalInfo empty_signal_info; + return empty_signal_info; + } else if (dist_to_original_desired_end < 0.0) { + return new_signal; + } else if (dist_to_new_desired_end < 0.0) { + return original_signal; + } + + if (dist_to_original_desired_start <= dist_to_new_desired_start) { + const auto enable_prior = use_prior_turn_signal( + dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, + dist_to_new_required_end); + + if (enable_prior) { + return original_signal; + } + return new_signal; + } + + const auto enable_prior = use_prior_turn_signal( + dist_to_new_required_start, dist_to_new_required_end, dist_to_original_required_start, + dist_to_original_required_end); + if (enable_prior) { + return new_signal; + } + return original_signal; +} + bool TurnSignalDecider::use_prior_turn_signal( const double dist_to_prior_required_start, const double dist_to_prior_required_end, const double dist_to_subsequent_required_start, const double dist_to_subsequent_required_end) diff --git a/planning/behavior_path_planner/src/utils/avoidance/util.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp similarity index 63% rename from planning/behavior_path_planner/src/utils/avoidance/util.cpp rename to planning/behavior_path_planner/src/utils/avoidance/utils.cpp index dada269f0f24e..de7732a1413ba 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/util.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/avoidance/util.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -31,7 +31,7 @@ #include #include -namespace behavior_path_planner +namespace behavior_path_planner::utils::avoidance { using motion_utils::calcLongitudinalOffsetPoint; @@ -69,308 +69,6 @@ geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygo } return ret; } - -boost::optional intersect( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - // calculate intersection point - const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); - if (det == 0.0) { - return {}; - } - - const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; - const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; - if (t < 0 || 1 < t || s < 0 || 1 < s) { - return {}; - } - - geometry_msgs::msg::Point intersect_point; - intersect_point.x = t * p1.x + (1.0 - t) * p2.x; - intersect_point.y = t * p1.y + (1.0 - t) * p2.y; - intersect_point.z = t * p1.z + (1.0 - t) * p2.z; - return intersect_point; -} - -boost::optional> intersectBound( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const std::vector & bound, const size_t seg_idx1, - const size_t seg_idx2) -{ - const size_t start_idx = - static_cast(std::max(0, static_cast(std::min(seg_idx1, seg_idx2)) - 5)); - const size_t end_idx = static_cast(std::min( - static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); - for (int i = start_idx; i < static_cast(end_idx); ++i) { - const auto intersect_point = intersect(p1, p2, bound.at(i), bound.at(i + 1)); - if (intersect_point) { - std::pair result; - result.first = static_cast(i); - result.second = intersect_point.get(); - return result; - } - } - return boost::none; -} - -PolygonPoint transformBoundFrenetCoordinate( - const std::vector & points, const geometry_msgs::msg::Point & point) -{ - const size_t seg_idx = motion_utils::findNearestSegmentIndex(points, point); - const double lon_dist_to_segment = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, point); - const double lat_dist = motion_utils::calcLateralOffset(points, point, seg_idx); - return PolygonPoint{point, seg_idx, lon_dist_to_segment, lat_dist}; -} - -std::vector generatePolygonInsideBounds( - const std::vector & bound, const std::vector & edge_points, - const bool is_object_right) -{ - std::vector full_polygon; - for (const auto & edge_point : edge_points) { - const auto polygon_point = transformBoundFrenetCoordinate(bound, edge_point); - full_polygon.push_back(polygon_point); - } - - std::vector inside_poly; - for (int i = 0; i < static_cast(full_polygon.size()); ++i) { - const auto & curr_poly = full_polygon.at(i); - const auto & prev_poly = full_polygon.at(i == 0 ? full_polygon.size() - 1 : i - 1); - - const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); - const bool is_prev_outside = prev_poly.is_outside_bounds(is_object_right); - - if (is_curr_outside && is_prev_outside) { - continue; - } - if (!is_curr_outside && !is_prev_outside) { - inside_poly.push_back(curr_poly); - continue; - } - - const auto intersection = intersectBound( - prev_poly.point, curr_poly.point, bound, prev_poly.bound_seg_idx, curr_poly.bound_seg_idx); - if (!intersection) { - continue; - } - const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment( - bound, intersection->first, intersection->second); - const auto intersect_point = - PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0}; - - if (is_prev_outside && !is_curr_outside) { - inside_poly.push_back(intersect_point); - inside_poly.push_back(curr_poly); - continue; - } - // Here is if (!is_prev_outside && is_curr_outside). - inside_poly.push_back(prev_poly); - inside_poly.push_back(intersect_point); - continue; - } - - return inside_poly; -} - -std::vector convertToGeometryPoints( - const std::vector & polygon_points) -{ - std::vector points; - points.reserve(polygon_points.size()); - - for (const auto & polygon_point : polygon_points) { - points.push_back(polygon_point.point); - } - return points; -} - -std::vector concatenateTwoPolygons( - const std::vector & front_polygon, const std::vector & back_polygon) -{ - // At first, the front polygon is the outside polygon - bool is_front_polygon_outside = true; - size_t outside_idx = 0; - - const auto get_out_poly = [&]() { - return is_front_polygon_outside ? front_polygon : back_polygon; - }; - const auto get_in_poly = [&]() { - return is_front_polygon_outside ? back_polygon : front_polygon; - }; - - std::vector concatenated_polygon; - while (rclcpp::ok()) { - concatenated_polygon.push_back(get_out_poly().at(outside_idx)); - if (outside_idx == get_out_poly().size() - 1) { - break; - } - const size_t curr_idx = outside_idx; - const size_t next_idx = outside_idx + 1; - - for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { - const auto intersection = intersect( - get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, - get_in_poly().at(i).point, get_in_poly().at(i + 1).point); - if (intersection) { - const auto intersect_point = PolygonPoint{intersection.get(), 0, 0.0, 0.0}; - concatenated_polygon.push_back(intersect_point); - - is_front_polygon_outside = !is_front_polygon_outside; - outside_idx = i; - break; - } - } - outside_idx += 1; - } - - return concatenated_polygon; -} - -std::vector> concatenatePolygons( - const std::vector> & polygons) -{ - auto unique_polygons = polygons; - - while (rclcpp::ok()) { - bool is_updated = false; - - for (size_t i = 0; i < unique_polygons.size(); ++i) { - for (size_t j = 0; j < i; ++j) { - const auto & p1 = unique_polygons.at(i); - const auto & p2 = unique_polygons.at(j); - - // if p1 and p2 overlaps - if (p1.back().is_after(p2.front()) && p2.back().is_after(p1.front())) { - is_updated = true; - - const auto concatenated_polygon = [&]() { - if (p2.front().is_after(p1.front())) { - return concatenateTwoPolygons(p1, p2); - } - return concatenateTwoPolygons(p2, p1); - }(); - - // NOTE: remove i's element first since is larger than j. - unique_polygons.erase(unique_polygons.begin() + i); - unique_polygons.erase(unique_polygons.begin() + j); - - unique_polygons.push_back(concatenated_polygon); - break; - } - } - if (is_updated) { - break; - } - } - - if (!is_updated) { - break; - } - } - return unique_polygons; -} - -std::vector getPolygonPointsInsideBounds( - const std::vector & bound, const std::vector & edge_points, - const bool is_object_right) -{ - // NOTE: Polygon is defined at lest by three points. - if (edge_points.size() < 3) { - return std::vector(); - } - - // convert to vector of PolygonPoint - const auto inside_polygon = [&]() { - auto tmp_polygon = generatePolygonInsideBounds(bound, edge_points, is_object_right); - - // In order to make the order of points the same as the order of lon_dist_to_segment. - // The order of points is clockwise. - if (!is_object_right) { - std::reverse(tmp_polygon.begin(), tmp_polygon.end()); - } - return tmp_polygon; - }(); - if (inside_polygon.empty()) { - return std::vector(); - } - - // search start and end index by longitudinal distance - std::vector polygon_indices(inside_polygon.size()); - std::iota(polygon_indices.begin(), polygon_indices.end(), 0); - std::sort(polygon_indices.begin(), polygon_indices.end(), [&](int i1, int i2) { - return inside_polygon.at(i2).is_after(inside_polygon.at(i1)); - }); - const int start_idx = polygon_indices.front(); - const int end_idx = polygon_indices.back(); - - // calculate valid inside polygon - std::vector valid_inside_polygon; - for (int i = 0; i < (end_idx - start_idx + static_cast(polygon_indices.size())) % - static_cast(polygon_indices.size()) + - 1; - ++i) { - const int poly_idx = (start_idx + i) % static_cast(inside_polygon.size()); - valid_inside_polygon.push_back(inside_polygon.at(poly_idx)); - } - - // add start and end points projected to bound if necessary - if (inside_polygon.at(start_idx).lat_dist_to_bound != 0.0) { // not on bound - auto start_point = inside_polygon.at(start_idx); - const auto start_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( - bound, start_point.bound_seg_idx, start_point.lon_dist_to_segment); - if (start_point_on_bound) { - start_point.point = start_point_on_bound.get(); - valid_inside_polygon.insert(valid_inside_polygon.begin(), start_point); - } - } - if (inside_polygon.at(end_idx).lat_dist_to_bound != 0.0) { // not on bound - auto end_point = inside_polygon.at(end_idx); - const auto end_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( - bound, end_point.bound_seg_idx, end_point.lon_dist_to_segment); - if (end_point_on_bound) { - end_point.point = end_point_on_bound.get(); - valid_inside_polygon.insert(valid_inside_polygon.end(), end_point); - } - } - return valid_inside_polygon; -} - -std::vector updateBoundary( - const std::vector & original_bound, - const std::vector> & sorted_polygons) -{ - if (sorted_polygons.empty()) { - return original_bound; - } - - auto reversed_polygons = sorted_polygons; - std::reverse(reversed_polygons.begin(), reversed_polygons.end()); - - auto updated_bound = original_bound; - - // NOTE: Further obstacle is applied first since a part of the updated_bound is erased. - for (const auto & polygon : reversed_polygons) { - const auto & start_poly = polygon.front(); - const auto & end_poly = polygon.back(); - - const double front_offset = motion_utils::calcLongitudinalOffsetToSegment( - updated_bound, start_poly.bound_seg_idx, start_poly.point); - - const size_t removed_start_idx = - 0 < front_offset ? start_poly.bound_seg_idx + 1 : start_poly.bound_seg_idx; - const size_t removed_end_idx = end_poly.bound_seg_idx; - - updated_bound.erase( - updated_bound.begin() + removed_start_idx, updated_bound.begin() + removed_end_idx + 1); - - const auto obj_points = convertToGeometryPoints(polygon); - updated_bound.insert( - updated_bound.begin() + removed_start_idx, obj_points.begin(), obj_points.end()); - } - return updated_bound; -} } // namespace bool isOnRight(const ObjectData & obj) @@ -398,6 +96,32 @@ double calcShiftLength( return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0; } +bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length) +{ + /** + * ^ + * | + * --+----x-------------------------------x---> + * | x x + * | ==obj== + */ + if (is_object_on_right && shift_length < 0.0) { + return false; + } + + /** + * ^ ==obj== + * | x x + * --+----x-------------------------------x---> + * | + */ + if (!is_object_on_right && shift_length > 0.0) { + return false; + } + + return true; +} + bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length) { return (is_object_on_right == std::signbit(shift_length)); @@ -462,10 +186,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & ap) void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj) { - const double distance = motion_utils::calcSignedArcLength( - path.points, ego_pos, obj.object.kinematics.initial_pose_with_covariance.pose.position); - double min_distance = distance; - double max_distance = distance; + double min_distance = std::numeric_limits::max(); + double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); @@ -574,23 +296,20 @@ Polygon2d createEnvelopePolygon( return expanded_polygon; } -void generateDrivableArea( - PathWithLaneId & path, const std::vector & lanes, - const std::shared_ptr planner_data, - const std::shared_ptr & parameters, const ObjectDataArray & objects, - const double vehicle_length, const bool enable_bound_clipping, const bool disable_path_update) +std::vector generateObstaclePolygonsForDrivableArea( + const ObjectDataArray & objects, const std::shared_ptr & parameters, + const double vehicle_width) { - utils::generateDrivableArea(path, lanes, vehicle_length, planner_data); - if (objects.empty() || !enable_bound_clipping) { - return; + std::vector obstacles_for_drivable_area; + + if (objects.empty() || !parameters->enable_bound_clipping) { + return obstacles_for_drivable_area; } - std::vector> right_polygons; - std::vector> left_polygons; for (const auto & object : objects) { // If avoidance is executed by both behavior and motion, only non-avoidable object will be // extracted from the drivable area. - if (!disable_path_update) { + if (!parameters->disable_path_update) { if (object.is_avoidable) { continue; } @@ -605,61 +324,15 @@ void generateDrivableArea( const auto object_parameter = parameters->object_parameters.at(t); // generate obstacle polygon - const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const double diff_poly_buffer = object.avoid_margin.get() - - object_parameter.envelope_buffer_margin - - planner_data->parameters.vehicle_width / 2.0; + const double diff_poly_buffer = + object.avoid_margin.get() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - - // get edge points of the object - const size_t nearest_path_idx = motion_utils::findNearestIndex( - path.points, obj_pose.position); // to get z for object polygon - std::vector edge_points; - for (size_t i = 0; i < obj_poly.outer().size() - 1; - ++i) { // NOTE: There is a duplicated points - edge_points.push_back(tier4_autoware_utils::createPoint( - obj_poly.outer().at(i).x(), obj_poly.outer().at(i).y(), - path.points.at(nearest_path_idx).point.pose.position.z)); - } - - // get a boundary that we have to change - const double lat_dist_to_path = motion_utils::calcLateralOffset(path.points, obj_pose.position); - const bool is_object_right = lat_dist_to_path < 0.0; - const auto & bound = is_object_right ? path.right_bound : path.left_bound; - - // get polygon points inside the bounds - const auto inside_polygon = getPolygonPointsInsideBounds(bound, edge_points, is_object_right); - if (!inside_polygon.empty()) { - if (is_object_right) { - right_polygons.push_back(inside_polygon); - } else { - left_polygons.push_back(inside_polygon); - } - } - } - - for (size_t i = 0; i < 2; ++i) { // for loop for right and left - const bool is_object_right = (i == 0); - const auto & polygons = is_object_right ? right_polygons : left_polygons; - if (polygons.empty()) { - continue; - } - - // concatenate polygons if they are longitudinal overlapped. - auto unique_polygons = concatenatePolygons(polygons); - - // sort bounds longitudinally - std::sort( - unique_polygons.begin(), unique_polygons.end(), - [](const std::vector & p1, const std::vector & p2) { - return p2.front().is_after(p1.front()); - }); - - // update boundary - auto & bound = is_object_right ? path.right_bound : path.left_bound; - bound = updateBoundary(bound, unique_polygons); + const bool is_left = 0 < object.lateral; + obstacles_for_drivable_area.push_back( + {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, is_left}); } + return obstacles_for_drivable_area; } double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) @@ -806,21 +479,28 @@ void fillObjectMovingTime( object_data.last_stop = now; object_data.move_time = 0.0; if (is_new_object) { + object_data.stop_time = 0.0; + object_data.last_move = now; stopped_objects.push_back(object_data); } else { + same_id_obj->stop_time = (now - same_id_obj->last_move).seconds(); same_id_obj->last_stop = now; same_id_obj->move_time = 0.0; + object_data.stop_time = same_id_obj->stop_time; } return; } if (is_new_object) { - object_data.move_time = std::numeric_limits::max(); + object_data.move_time = std::numeric_limits::infinity(); + object_data.stop_time = 0.0; + object_data.last_move = now; return; } object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (now - same_id_obj->last_stop).seconds(); + object_data.stop_time = 0.0; if (object_data.move_time > parameters->threshold_time_object_is_moving) { stopped_objects.erase(same_id_obj); @@ -929,7 +609,7 @@ void filterTargetObjects( using lanelet::utils::to2D; const auto & rh = planner_data->route_handler; - const auto & path_points = data.reference_path.points; + const auto & path_points = data.reference_path_rough.points; const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto & vehicle_width = planner_data->parameters.vehicle_width; const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); @@ -959,6 +639,9 @@ void filterTargetObjects( continue; } + // calc longitudinal distance from ego to closest target object footprint point. + fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, o); + // object is behind ego or too far. if (o.longitudinal < -parameters->object_check_backward_distance) { o.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; @@ -993,21 +676,58 @@ void filterTargetObjects( o.overhang_lanelet = overhang_lanelet; lanelet::BasicPoint3d overhang_basic_pose( o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z); + const bool get_left = isOnRight(o) && parameters->enable_avoidance_over_same_direction; const bool get_right = !isOnRight(o) && parameters->enable_avoidance_over_same_direction; + const bool get_opposite = parameters->enable_avoidance_over_opposite_direction; - const auto target_lines = rh->getFurthestLinestring( - overhang_lanelet, get_right, get_left, - parameters->enable_avoidance_over_opposite_direction); + lanelet::ConstLineString3d target_line{}; + { + const auto lines = + rh->getFurthestLinestring(overhang_lanelet, get_right, get_left, get_opposite); + if (isOnRight(o)) { + o.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString())); + debug.bounds.push_back(lines.back()); + } else { + o.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString())); + debug.bounds.push_back(lines.front()); + } + } - if (isOnRight(o)) { - o.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); - debug.bounds.push_back(target_lines.back()); - } else { - o.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); - debug.bounds.push_back(target_lines.front()); + lanelet::ConstLanelets previous_lanelet{}; + if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) { + const auto lines = + rh->getFurthestLinestring(previous_lanelet.front(), get_right, get_left, get_opposite); + if (isOnRight(o)) { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.back()); + } else { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.front()); + } + } + + lanelet::ConstLanelet next_lanelet{}; + if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { + const auto lines = + rh->getFurthestLinestring(next_lanelet, get_right, get_left, get_opposite); + if (isOnRight(o)) { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.back()); + } else { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.front()); + } } } @@ -1027,18 +747,50 @@ void filterTargetObjects( return std::min(max_allowable_lateral_distance, max_avoid_margin); }(); - // force avoidance for stopped vehicle - { + if (!!avoid_margin) { + const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, avoid_margin.get()); + if (!isShiftNecessary(isOnRight(o), shift_length)) { + o.reason = "NotNeedAvoidance"; + data.other_objects.push_back(o); + continue; + } + + if (std::abs(shift_length) < parameters->lateral_execution_threshold) { + o.reason = "LessThanExecutionThreshold"; + data.other_objects.push_back(o); + continue; + } + } + + const auto stop_time_longer_than_threshold = + o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; + + if (stop_time_longer_than_threshold && parameters->enable_force_avoidance_for_stopped_vehicle) { + // force avoidance for stopped vehicle + bool not_parked_object = true; + + // check traffic light const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, data.current_lanelets); + { + not_parked_object = to_traffic_light < parameters->object_ignore_distance_traffic_light; + } - o.to_stop_factor_distance = std::min(to_traffic_light, o.to_stop_factor_distance); - } + // check crosswalk + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto to_crosswalk = + utils::getDistanceToCrosswalk(ego_pose, data.current_lanelets, *rh->getOverallGraphPtr()) - + o.longitudinal; + { + const auto stop_for_crosswalk = + to_crosswalk < parameters->object_ignore_distance_crosswalk_forward && + to_crosswalk > -1.0 * parameters->object_ignore_distance_crosswalk_backward; + not_parked_object = not_parked_object || stop_for_crosswalk; + } - if ( - o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle && - parameters->enable_force_avoidance_for_stopped_vehicle) { - if (o.to_stop_factor_distance > parameters->object_check_force_avoidance_clearance) { + o.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); + + if (!not_parked_object) { o.last_seen = now; o.avoid_margin = avoid_margin; data.target_objects.push_back(o); @@ -1053,13 +805,6 @@ void filterTargetObjects( continue; } - lanelet::ConstLanelet object_closest_lanelet; - const auto lanelet_map = rh->getLaneletMapPtr(); - if (!lanelet::utils::query::getClosestLanelet( - lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) { - continue; - } - lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y()); /** @@ -1085,7 +830,7 @@ void filterTargetObjects( } const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position); + lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position); lanelet::BasicPoint3d centerline_point( centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); @@ -1100,9 +845,9 @@ void filterTargetObjects( // o: nearest point on centerline bool is_left_side_parked_vehicle = false; - { + if (!isOnRight(o)) { auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); + const auto most_left_road_lanelet = rh->getMostLeftLanelet(overhang_lanelet); const auto most_left_lanelet_candidates = rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); @@ -1128,17 +873,17 @@ void filterTargetObjects( object_shiftable_distance += parameters->object_check_min_road_shoulder_width; } - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + const auto arc_coordinates = + toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid); o.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; } bool is_right_side_parked_vehicle = false; - { + if (isOnRight(o)) { auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_right_road_lanelet = rh->getMostRightLanelet(object_closest_lanelet); + const auto most_right_road_lanelet = rh->getMostRightLanelet(overhang_lanelet); const auto most_right_lanelet_candidates = rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); @@ -1164,8 +909,8 @@ void filterTargetObjects( object_shiftable_distance += parameters->object_check_min_road_shoulder_width; } - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + const auto arc_coordinates = + toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid); o.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -1185,4 +930,4 @@ void filterTargetObjects( data.target_objects.push_back(o); } } -} // namespace behavior_path_planner +} // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 43d2b50a29de4..fb99d04f89e91 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -123,9 +123,11 @@ std::vector GeometricParallelParking::generatePullOverPaths( const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin : parameters_.backward_parking_lane_departure_margin; + const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval + : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - lane_departure_margin); + lane_departure_margin, arc_path_interval); if (arc_paths.empty()) { return std::vector{}; } @@ -182,7 +184,7 @@ bool GeometricParallelParking::planPullOver( constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; - for (double steer = parameters_.max_steer_angle; steer > min_steer_rad; + for (double steer = parameters_.forward_parking_max_steer_angle; steer > min_steer_rad; steer -= steer_interval) { const double R_E_r = common_params.wheel_base / std::tan(steer); const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward); @@ -228,14 +230,14 @@ bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes) { - constexpr bool is_forward = false; // parking backward means departing forward + constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose constexpr double max_offset = 10.0; constexpr double offset_interval = 1.0; for (double end_pose_offset = 0; end_pose_offset < max_offset; end_pose_offset += offset_interval) { - // departing end pose which is the second arc path end + // pull_out end pose which is the second arc path end const auto end_pose = calcStartPose(start_pose, end_pose_offset, R_E_min_, is_forward); if (!end_pose) { continue; @@ -244,7 +246,7 @@ bool GeometricParallelParking::planPullOut( // plan reverse path of parking. end_pose <-> start_pose auto arc_paths = planOneTrial( *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, start_pose_offset, - parameters_.departing_lane_departure_margin); + parameters_.pull_out_lane_departure_margin, parameters_.pull_out_path_interval); if (arc_paths.empty()) { // not found path continue; @@ -265,7 +267,7 @@ bool GeometricParallelParking::planPullOut( } } - // get road center line path from departing end to goal, and combine after the second arc path + // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; const double road_lanes_length = std::accumulate( @@ -287,9 +289,9 @@ bool GeometricParallelParking::planPullOut( continue; } - // set departing velocity to arc paths and 0 velocity to end point + // set pull_out velocity to arc paths and 0 velocity to end point constexpr bool set_stop_end = false; - setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end); + setVelocityToArcPaths(arc_paths, parameters_.pull_out_velocity, set_stop_end); arc_paths.back().points.front().point.longitudinal_velocity_mps = 0.0; // combine the road center line path with the second arc path @@ -350,7 +352,9 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start auto path = planner_data_->route_handler->getCenterLinePath( current_lanes, current_arc_position.length, start_arc_position.length, true); path.header = planner_data_->route_handler->getRouteHeader(); - path.points.back().point.longitudinal_velocity_mps = 0; + if (!path.points.empty()) { + path.points.back().point.longitudinal_velocity_mps = 0; + } return path; } @@ -358,7 +362,8 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start std::vector GeometricParallelParking::planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_r, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double lane_departure_margin) + const bool is_forward, const double end_pose_offset, const double lane_departure_margin, + const double arc_path_interval) { clearPaths(); @@ -403,7 +408,7 @@ std::vector GeometricParallelParking::planOneTrial( const double R_front_left = std::hypot(R_E_r + common_params.vehicle_width / 2, common_params.base_link2front); const double distance_to_left_bound = - utils::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, arc_end_pose); + utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, true); const double left_deviation = R_front_left - R_E_r; if (std::abs(distance_to_left_bound) - left_deviation < lane_departure_margin) { return std::vector{}; @@ -413,7 +418,7 @@ std::vector GeometricParallelParking::planOneTrial( std::hypot(R_E_l + common_params.vehicle_width / 2, common_params.base_link2front); const double right_deviation = R_front_right - R_E_l; const double distance_to_right_bound = - utils::getSignedDistanceFromRightBoundary(lanes, start_pose); + utils::getSignedDistanceFromBoundary(lanes, start_pose, false); if (distance_to_right_bound - right_deviation < lane_departure_margin) { return std::vector{}; } @@ -426,12 +431,14 @@ std::vector GeometricParallelParking::planOneTrial( (2 * R_E_l * (R_E_l + R_E_r))); theta_l = is_forward ? theta_l : -theta_l; - PathWithLaneId path_turn_left = - generateArcPath(Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), is_forward, is_forward); + PathWithLaneId path_turn_left = generateArcPath( + Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, + is_forward); path_turn_left.header = planner_data_->route_handler->getRouteHeader(); PathWithLaneId path_turn_right = generateArcPath( - Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); + Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, + is_forward); path_turn_right.header = planner_data_->route_handler->getRouteHeader(); auto setLaneIds = [lanes](PathPointWithLaneId & p) { @@ -473,11 +480,12 @@ std::vector GeometricParallelParking::planOneTrial( PathWithLaneId GeometricParallelParking::generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, + const double arc_path_interval, const bool is_left_turn, // is_left_turn means clockwise around center. const bool is_forward) { PathWithLaneId path; - const double yaw_interval = parameters_.arc_path_interval / radius; + const double yaw_interval = arc_path_interval / radius; double yaw = start_yaw; if (is_left_turn) { if (end_yaw < start_yaw) end_yaw += M_PI_2; @@ -530,18 +538,13 @@ PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( return p; } -void GeometricParallelParking::setData( - const std::shared_ptr & planner_data, - const ParallelParkingParameters & parameters) +void GeometricParallelParking::setTurningRadius( + const BehaviorPathPlannerParameters & common_params, const double max_steer_angle) { - planner_data_ = planner_data; - parameters_ = parameters; - - auto common_params = planner_data_->parameters; - - R_E_min_ = common_params.wheel_base / std::tan(parameters_.max_steer_angle); + R_E_min_ = common_params.wheel_base / std::tan(max_steer_angle); R_Bl_min_ = std::hypot( R_E_min_ + common_params.wheel_tread / 2 + common_params.left_over_hang, common_params.wheel_base + common_params.front_overhang); } + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp new file mode 100644 index 0000000000000..085f72b59d31e --- /dev/null +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -0,0 +1,99 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" + +#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_utils.hpp" + +#include +#include + +#include +#include + +namespace behavior_path_planner +{ + +BehaviorModuleOutput DefaultFixedGoalPlanner::plan( + const std::shared_ptr & planner_data) const +{ + BehaviorModuleOutput output = +#ifdef USE_OLD_ARCHITECTURE + // generate reference path in this planner + std::invoke([this, &planner_data]() { + auto path = getReferencePath(planner_data); + if (!path) { + return BehaviorModuleOutput{}; + } + return *path; + }); +#else + // use planner previous module reference path + getPreviousModuleOutput(); +#endif + const PathWithLaneId smoothed_path = + modifyPathForSmoothGoalConnection(*(output.path), planner_data); + output.path = std::make_shared(smoothed_path); + output.reference_path = std::make_shared(smoothed_path); + return output; +} + +#ifdef USE_OLD_ARCHITECTURE +boost::optional DefaultFixedGoalPlanner::getReferencePath( + const std::shared_ptr & planner_data) const +{ + const auto & route_handler = planner_data->route_handler; + const auto current_pose = planner_data->self_odometry->pose.pose; + const double dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; + const double yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; + + lanelet::ConstLanelet current_lane{}; + if (route_handler->getClosestLaneletWithConstrainsWithinRoute( + current_pose, ¤t_lane, dist_threshold, yaw_threshold)) { + return utils::getReferencePath(current_lane, planner_data); + } + + if (route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { + return utils::getReferencePath(current_lane, planner_data); + } + + return {}; // something wrong +} +#endif + +PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( + const PathWithLaneId & path, const std::shared_ptr & planner_data) const +{ + const auto goal = planner_data->route_handler->getGoalPose(); + const auto goal_lane_id = planner_data->route_handler->getGoalLaneId(); + + Pose refined_goal{}; + { + lanelet::ConstLanelet goal_lanelet; + if (planner_data->route_handler->getGoalLanelet(&goal_lanelet)) { + refined_goal = utils::refineGoal(goal, goal_lanelet); + } else { + refined_goal = goal; + } + } + + auto refined_path = utils::refinePathForGoal( + planner_data->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal, + goal_lane_id); + + return refined_path; +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp similarity index 83% rename from planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 8bb450f60e5fb..a3e710b853503 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include @@ -23,20 +23,21 @@ namespace behavior_path_planner { FreespacePullOver::FreespacePullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); - if (parameters.algorithm == "astar") { + if (parameters.freespace_parking_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( - parameters.common_parameters, vehicle_shape, parameters.astar_parameters); - } else if (parameters.algorithm == "rrtstar") { + parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_parking_algorithm == "rrtstar") { use_back_ = true; // no option for disabling back in rrtstar planner_ = std::make_unique( - parameters.common_parameters, vehicle_shape, parameters.rrt_star_parameters); + parameters.freespace_parking_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); } } @@ -109,9 +110,12 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) // path points is less than 2 return {}; } + + // for old architecture + // NOTE: drivable_area_info is assigned outside this function. + const double offset = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; utils::generateDrivableArea( - path, planner_data_->parameters.vehicle_length, planner_data_->parameters.vehicle_width, - drivable_area_margin, *is_driving_forward); + path, planner_data_->parameters.vehicle_length, offset, *is_driving_forward); } PullOverPath pull_over_path{}; diff --git a/planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp similarity index 75% rename from planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 97a4cd9055709..ed38d09f879ba 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include @@ -26,17 +26,17 @@ namespace behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr occupancy_grid_map, const bool is_forward) : PullOverPlannerBase{node, parameters}, - parallel_parking_parameters_{parallel_parking_parameters}, + parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{lane_departure_checker}, occupancy_grid_map_{occupancy_grid_map}, is_forward_{is_forward} { + planner_.setParameters(parallel_parking_parameters_); } boost::optional GeometricPullOver::plan(const Pose & goal_pose) @@ -45,15 +45,20 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); + const auto shoulder_lanes = + goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } auto lanes = road_lanes; lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - // todo: set param only once - planner_.setData(planner_data_, parallel_parking_parameters_); + const auto & p = parallel_parking_parameters_; + const double max_steer_angle = + is_forward_ ? p.forward_parking_max_steer_angle : p.backward_parking_max_steer_angle; + planner_.setTurningRadius(planner_data_->parameters, max_steer_angle); + planner_.setPlannerData(planner_data_); + const bool found_valid_path = planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); if (!found_valid_path) { diff --git a/planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp similarity index 84% rename from planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 54fc50c2cb860..d35842d09f056 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/goal_searcher.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include @@ -31,11 +31,12 @@ using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; GoalSearcher::GoalSearcher( - const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map) : GoalSearcherBase{parameters}, vehicle_footprint_{vehicle_footprint}, - occupancy_grid_map_{occupancy_grid_map} + occupancy_grid_map_{occupancy_grid_map}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } @@ -51,7 +52,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; - const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*route_handler); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); auto lanes = utils::getExtendedCurrentLanes(planner_data_); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); @@ -78,12 +80,14 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - const auto distance_from_left_bound = utils::getSignedDistanceFromShoulderLeftBoundary( - pull_over_lanes, vehicle_footprint_, center_pose); + const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); if (!distance_from_left_bound) continue; - const double offset_from_center_line = distance_from_left_bound.value() + margin_from_boundary; - const Pose original_search_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + const double sign = left_side_parking_ ? 1.0 : -1.0; + const double offset_from_center_line = + sign * (std::abs(distance_from_left_bound.value()) - margin_from_boundary); + const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( center_line_path.points, original_goal_pose.position, original_search_pose.position)); @@ -93,7 +97,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_search_pose, 0, -dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, -sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); @@ -137,11 +141,12 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } // check margin with pull over lane objects - const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); const auto [shoulder_lane_objects, others] = utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; - const auto target_objects = pull_over_utils::filterObjectsByLateralDistance( + const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { @@ -240,18 +245,25 @@ void GoalSearcher::createAreaPolygons(std::vector original_search_poses) boost::geometry::clear(area_polygons_); for (const auto p : original_search_poses) { Polygon2d footprint{}; - const Point p_left_front = calcOffsetPose(p, base_link2front, vehicle_width / 2, 0).position; + + const double left_front_offset = + left_side_parking_ ? vehicle_width / 2 : vehicle_width / 2 + max_lateral_offset; + const Point p_left_front = calcOffsetPose(p, base_link2front, left_front_offset, 0).position; appendPointToPolygon(footprint, p_left_front); - const Point p_right_front = - calcOffsetPose(p, base_link2front, -vehicle_width / 2 - max_lateral_offset, 0).position; + const double right_front_offset = + left_side_parking_ ? -vehicle_width / 2 - max_lateral_offset : -vehicle_width / 2; + const Point p_right_front = calcOffsetPose(p, base_link2front, right_front_offset, 0).position; appendPointToPolygon(footprint, p_right_front); - const Point p_right_back = - calcOffsetPose(p, -base_link2rear, -vehicle_width / 2 - max_lateral_offset, 0).position; + const double right_back_offset = + left_side_parking_ ? -vehicle_width / 2 - max_lateral_offset : -vehicle_width / 2; + const Point p_right_back = calcOffsetPose(p, -base_link2rear, right_back_offset, 0).position; appendPointToPolygon(footprint, p_right_back); - const Point p_left_back = calcOffsetPose(p, -base_link2rear, vehicle_width / 2, 0).position; + const double left_back_offset = + left_side_parking_ ? vehicle_width / 2 : vehicle_width / 2 + max_lateral_offset; + const Point p_left_back = calcOffsetPose(p, -base_link2rear, left_back_offset, 0).position; appendPointToPolygon(footprint, p_left_back); appendPointToPolygon(footprint, p_left_front); diff --git a/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp similarity index 90% rename from planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index d0bd5d00447b7..073e97e01912e 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/shift_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include @@ -26,12 +26,13 @@ namespace behavior_path_planner { ShiftPullOver::ShiftPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr & occupancy_grid_map) : PullOverPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}, - occupancy_grid_map_{occupancy_grid_map} + occupancy_grid_map_{occupancy_grid_map}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } boost::optional ShiftPullOver::plan(const Pose & goal_pose) @@ -39,12 +40,13 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto & route_handler = planner_data_->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; const double max_jerk = parameters_.maximum_lateral_jerk; - const int pull_over_sampling_num = parameters_.pull_over_sampling_num; - const double jerk_resolution = std::abs(max_jerk - min_jerk) / pull_over_sampling_num; + const int shift_sampling_num = parameters_.shift_sampling_num; + const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; // get road and shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); + const auto shoulder_lanes = + goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } @@ -97,10 +99,10 @@ boost::optional ShiftPullOver::generatePullOverPath( const Pose & goal_pose, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; - const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; + const double after_shift_straight_distance = parameters_.after_shift_straight_distance; const Pose shift_end_pose = - tier4_autoware_utils::calcOffsetPose(goal_pose, -after_pull_over_straight_distance, 0, 0); + tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); // generate road lane reference path to shift end const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( @@ -134,6 +136,8 @@ boost::optional ShiftPullOver::generatePullOverPath( ShiftedPath shifted_path{}; const bool offset_back = true; // offset front side from reference path if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); + motion_utils::insertOrientation(shifted_path.path.points, true); // set same orientation, because the reference center line orientation is not same to the shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation; @@ -204,9 +208,10 @@ boost::optional ShiftPullOver::generatePullOverPath( // check if the parking path will leave lanes const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); if (lane_departure_checker_.checkPathWillLeaveLane( utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { return {}; diff --git a/planning/behavior_path_planner/src/utils/pull_over/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp similarity index 82% rename from planning/behavior_path_planner/src/utils/pull_over/util.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/util.cpp index c1253d7ecaa10..cc44e7f0e012d 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/util.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" @@ -42,7 +42,7 @@ using tier4_autoware_utils::createPoint; namespace behavior_path_planner { -namespace pull_over_utils +namespace goal_planner_utils { PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) { @@ -55,19 +55,36 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith return path; } -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler) +lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) { - lanelet::ConstLanelets pull_over_lanes; - lanelet::ConstLanelet target_shoulder_lane; const Pose goal_pose = route_handler.getGoalPose(); + lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { - constexpr double pull_over_lane_length = 200; - pull_over_lanes = route_handler.getShoulderLaneletSequence( - target_shoulder_lane, goal_pose, pull_over_lane_length, pull_over_lane_length); + // pull over on shoulder lane + return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose); } - return pull_over_lanes; + + // pull over on road lane + lanelet::ConstLanelet closest_lane{}; + route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane); + + lanelet::ConstLanelet outermost_lane{}; + if (left_side) { + outermost_lane = route_handler.getMostLeftLanelet(closest_lane); + } else { + outermost_lane = route_handler.getMostRightLanelet(closest_lane); + } + + lanelet::ConstLanelet outermost_shoulder_lane; + if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) { + return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose); + } + + const bool dist = std::numeric_limits::max(); + constexpr bool only_route_lanes = false; + return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes); } PredictedObjects filterObjectsByLateralDistance( @@ -163,5 +180,5 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -} // namespace pull_over_utils +} // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 5ea200b741a9f..3145bc4ac7394 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -38,7 +38,7 @@ #include #include -namespace +namespace behavior_path_planner::utils::lane_change { using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -49,80 +49,85 @@ using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) -{ - std::vector> sorted_lane_ids{}; - sorted_lane_ids.reserve(target_lanes.size()); - const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { - const auto routing_graph_ptr = route_handler.getRoutingGraphPtr(); - lanelet::ConstLanelet lane; - if (rough_shift_length < 0.0) { - // lane change to the left, so I wan to take the lane right to target - const auto has_target_right = routing_graph_ptr->right(target_lane); - if (has_target_right) { - lane = *has_target_right; - } - } else if (rough_shift_length > 0.0) { - const auto has_target_left = routing_graph_ptr->left(target_lane); - if (has_target_left) { - lane = *has_target_left; - } - } else { - lane = target_lane; - } - - const auto find_same_id = std::find_if( - current_lanes.cbegin(), current_lanes.cend(), - [&lane](const lanelet::ConstLanelet & orig) { return orig.id() == lane.id(); }); +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using lanelet::ArcCoordinates; - if (find_same_id == current_lanes.cend()) { - return std::vector{target_lane.id()}; - } +double calcLaneChangeResampleInterval( + const double lane_changing_length, const double lane_changing_velocity) +{ + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + return std::max( + lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); +} - if (target_lane.id() > find_same_id->id()) { - return std::vector{find_same_id->id(), target_lane.id()}; - } +double calcMaximumAcceleration( + const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, + const BehaviorPathPlannerParameters & params) +{ + const double & max_acc = params.max_acc; + if (path.points.empty()) { + return max_acc; + } - return std::vector{target_lane.id(), find_same_id->id()}; - }; + const double & nearest_dist_threshold = params.ego_nearest_dist_threshold; + const double & nearest_yaw_threshold = params.ego_nearest_yaw_threshold; - std::transform( - target_lanes.cbegin(), target_lanes.cend(), std::back_inserter(sorted_lane_ids), - get_sorted_lane_ids); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double & max_path_velocity = + path.points.at(current_seg_idx).point.longitudinal_velocity_mps; + const double & prepare_duration = params.lane_change_prepare_duration; - return sorted_lane_ids; + const double acc = (max_path_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, 0.0, max_acc); } -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids) +void setPrepareVelocity( + PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { - for (const auto original_id : original_lane_ids) { - for (const auto & sorted_id : sorted_lane_ids) { - if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { - return sorted_id; - } + if (current_velocity < prepare_velocity) { + // acceleration + for (size_t i = 0; i < prepare_segment.points.size(); ++i) { + prepare_segment.points.at(i).point.longitudinal_velocity_mps = std::min( + prepare_segment.points.at(i).point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); } + } else { + // deceleration + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); } - return original_lane_ids; } -} // namespace -namespace behavior_path_planner::utils::lane_change +std::vector getAccelerationValues( + const double min_acc, const double max_acc, const size_t sampling_num) { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using lanelet::ArcCoordinates; -using utils::getHighestProbLabel; + if (min_acc > max_acc) { + return {}; + } -inline double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity) -{ - constexpr auto min_resampling_points{30.0}; - constexpr auto resampling_dt{0.2}; - return std::max( - lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); + if (max_acc - min_acc < std::numeric_limits::epsilon()) { + return {0.0}; + } + + constexpr double epsilon = 0.001; + const auto resolution = std::abs(max_acc - min_acc) / sampling_num; + + std::vector sampled_values{min_acc}; + for (double sampled_acc = min_acc + resolution; + sampled_acc < max_acc + std::numeric_limits::epsilon(); sampled_acc += resolution) { + // check whether if we need to add 0.0 + if (sampled_values.back() < -epsilon && sampled_acc > epsilon) { + sampled_values.push_back(0.0); + } + + sampled_values.push_back(sampled_acc); + } + std::reverse(sampled_values.begin(), sampled_values.end()); + + return sampled_values; } PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) @@ -164,8 +169,10 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const std::vector> & sorted_lane_ids, const double acceleration, - const LaneChangePhaseInfo lane_change_length, const LaneChangePhaseInfo lane_change_velocity, + const std::vector> & sorted_lane_ids, const double longitudinal_acceleration, + const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length, + const LaneChangePhaseInfo lane_change_velocity, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; @@ -177,9 +184,8 @@ std::optional constructCandidatePath( bool offset_back = false; const auto lane_changing_velocity = lane_change_velocity.lane_changing; - path_shifter.setVelocity(lane_changing_velocity); - path_shifter.setLateralAccelerationLimit(std::abs(lane_change_param.lane_changing_lateral_acc)); + path_shifter.setLateralAccelerationLimit(std::abs(lateral_acceleration)); if (!path_shifter.generate(&shifted_path, offset_back)) { RCLCPP_DEBUG( @@ -191,12 +197,12 @@ std::optional constructCandidatePath( const auto & lane_changing_length = lane_change_length.lane_changing; LaneChangePath candidate_path; - candidate_path.acceleration = acceleration; + candidate_path.acceleration = longitudinal_acceleration; candidate_path.length.prepare = prepare_length; candidate_path.length.lane_changing = lane_changing_length; candidate_path.duration.prepare = std::invoke([&]() { const auto duration = prepare_length / lane_change_velocity.prepare; - return std::min(duration, lane_change_param.prepare_duration); + return std::min(duration, common_parameter.lane_change_prepare_duration); }); candidate_path.duration.lane_changing = std::invoke([&]() { const auto rounding_multiplier = 1.0 / lane_change_param.prediction_time_resolution; @@ -236,15 +242,18 @@ std::optional constructCandidatePath( lane_changing_start_point.point.longitudinal_velocity_mps); continue; } - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_velocity)); const auto nearest_idx = motion_utils::findNearestIndex(target_segment.points, point.point.pose); point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; } + // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster + const bool enable_path_check_in_lanelet = false; + // check candidate path is in lanelet - if (!isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { + if ( + enable_path_check_in_lanelet && + !isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { return std::nullopt; } @@ -254,283 +263,35 @@ std::optional constructCandidatePath( return std::optional{candidate_path}; } -#ifdef USE_OLD_ARCHITECTURE -bool getLaneChangePaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data) -#else -bool getLaneChangePaths( - const PathWithLaneId & original_path, const RouteHandler & route_handler, - const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, const Direction direction, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data) -#endif -{ - debug_data->clear(); - if (original_lanelets.empty() || target_lanelets.empty()) { - return false; - } - - Pose ego_pose_before_collision{}; - - // rename parameter - const auto backward_path_length = common_parameter.backward_path_length; - const auto forward_path_length = common_parameter.forward_path_length; - const auto prepare_duration = parameter.prepare_duration; - const auto minimum_prepare_length = common_parameter.minimum_prepare_length; - const auto minimum_lane_changing_velocity = parameter.minimum_lane_changing_velocity; - const auto lane_change_sampling_num = parameter.lane_change_sampling_num; - - // get velocity - const auto current_velocity = twist.linear.x; - - // compute maximum_deceleration - const auto maximum_deceleration = - std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, ¶meter]() { - const double min_a = - (minimum_lane_changing_velocity - current_velocity) / parameter.prepare_duration; - return std::clamp( - min_a, -std::abs(parameter.maximum_deceleration), -std::numeric_limits::epsilon()); - }); - - const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; - - const auto target_length = - utils::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); - -#ifdef USE_OLD_ARCHITECTURE - const auto num_to_preferred_lane = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); -#else - const auto get_opposite_direction = - (direction == Direction::RIGHT) ? Direction::LEFT : Direction::RIGHT; - const auto num_to_preferred_lane = std::abs( - route_handler.getNumLaneToPreferredLane(target_lanelets.back(), get_opposite_direction)); -#endif - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); - - const auto required_total_min_length = - utils::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); - - const auto dist_to_end_of_current_lanes = - utils::getDistanceToEndOfLane(pose, original_lanelets) - required_total_min_length; - - [[maybe_unused]] const auto arc_position_from_current = - lanelet::utils::getArcCoordinates(original_lanelets, pose); - const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); - - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); - - const auto sorted_lane_ids = getSortedLaneIds( - route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); - const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); - - LaneChangeTargetObjectIndices dynamic_object_indices; - - candidate_paths->reserve(lane_change_sampling_num); - for (double sampled_acc = 0.0; sampled_acc >= maximum_deceleration; - sampled_acc -= acceleration_resolution) { - const auto prepare_velocity = - std::max(current_velocity + sampled_acc * prepare_duration, minimum_lane_changing_velocity); - - // compute actual acceleration - const double acceleration = (prepare_velocity - current_velocity) / prepare_duration; - - // get path on original lanes - const double prepare_length = std::max( - current_velocity * prepare_duration + 0.5 * acceleration * std::pow(prepare_duration, 2), - minimum_prepare_length); - - if (prepare_length < target_length) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare length is shorter than distance to target lane!!"); - break; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto prepare_segment = getPrepareSegment( - route_handler, original_lanelets, arc_position_from_current.length, backward_path_length, - prepare_length, prepare_velocity); -#else - const auto prepare_segment = getPrepareSegment( - original_path, original_lanelets, pose, backward_path_length, prepare_length, - std::max(prepare_velocity, minimum_lane_changing_velocity)); -#endif - - if (prepare_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare segment is empty!!"); - continue; - } - - // lane changing start pose is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - original_lanelets, target_lanelets.front(), lane_changing_start_pose); - // In new architecture, there is a possibility that the lane change start pose is behind of the - // target lanelet, even if the condition prepare_length > target_length is satisfied. In - // that case, the lane change shouldn't be executed. - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "[only new arch] lane change start pose is behind target lanelet!!"); - break; - } - - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); - - // we assume constant velocity during lane change - const auto lane_changing_velocity = prepare_velocity; - const auto lane_changing_length = - calcLaneChangingLength(lane_changing_velocity, shift_length, common_parameter, parameter); - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "lane changing path too long"); - continue; - } - - if (is_goal_in_route) { - const double s_start = - lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length; - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; - if ( - s_start + lane_changing_length + parameter.lane_change_finish_judge_buffer + - required_total_min_length > - s_goal) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "length of lane changing path is longer than length to goal!!"); - continue; - } - } - - const auto target_segment = getTargetSegment( - route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, - target_lane_length, lane_changing_length, lane_changing_velocity, required_total_min_length); - - if (target_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target segment is empty!! something wrong..."); - continue; - } - - const auto resample_interval = - calcLaneChangeResampleInterval(lane_changing_length, lane_changing_velocity); - - const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - const auto target_lane_reference_path = getReferencePathFromTargetLane( - route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, - lc_length.lane_changing, forward_path_length, resample_interval, is_goal_in_route); - - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target_lane_reference_path is empty!!"); - continue; - } - - const auto shift_line = getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); - - const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity}; - - const auto candidate_path = constructCandidatePath( - prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, - target_lanelets, sorted_lane_ids, acceleration, lc_length, lc_velocity, parameter); - - if (!candidate_path) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "no candidate path!!"); - continue; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto is_valid = hasEnoughLength( - *candidate_path, original_lanelets, target_lanelets, pose, route_handler, - common_parameter.minimum_lane_changing_length); -#else - const auto is_valid = hasEnoughLength( - *candidate_path, original_lanelets, target_lanelets, pose, route_handler, - common_parameter.minimum_lane_changing_length, direction); -#endif - - if (!is_valid) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "invalid candidate path!!"); - continue; - } - - if (candidate_paths->empty()) { - // only compute dynamic object indices once - const auto backward_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - route_handler, target_lanelets.front(), pose, check_length); - dynamic_object_indices = filterObjectIndices( - {*candidate_path}, *dynamic_objects, backward_lanes, pose, - common_parameter.forward_path_length, parameter, lateral_buffer); - } - candidate_paths->push_back(*candidate_path); - - const auto is_safe = isLaneChangePathSafe( - *candidate_path, dynamic_objects, dynamic_object_indices, pose, twist, common_parameter, - parameter, common_parameter.expected_front_deceleration, - common_parameter.expected_rear_deceleration, ego_pose_before_collision, *debug_data, - acceleration); - - if (is_safe) { - return true; - } - } - - return false; -} - -#ifdef USE_OLD_ARCHITECTURE bool hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const RouteHandler & route_handler, const double minimum_lane_change_length) -#else -bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const RouteHandler & route_handler, const double minimum_lane_change_length, - const Direction direction) -#endif + const RouteHandler & route_handler, const double minimum_lane_changing_velocity, + const BehaviorPathPlannerParameters & common_parameter, const Direction direction) { - const double lane_change_total_length = path.length.sum(); -#ifdef USE_OLD_ARCHITECTURE - const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back())); -#else - const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); -#endif - const auto overall_graphs = route_handler.getOverallGraphPtr(); - - const double lane_change_required_length = static_cast(num) * minimum_lane_change_length; + const double lane_change_length = path.length.sum(); + const double & lateral_jerk = common_parameter.lane_changing_lateral_jerk; + const double max_lat_acc = + common_parameter.lane_change_lat_acc_map.find(minimum_lane_changing_velocity).second; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); + + double minimum_lane_change_length_to_preferred_lane = 0.0; + for (const auto & shift_length : shift_intervals) { + const auto lane_changing_time = + PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, max_lat_acc); + minimum_lane_change_length_to_preferred_lane += + minimum_lane_changing_velocity * lane_changing_time + common_parameter.minimum_prepare_length; + } - if (lane_change_total_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } const auto goal_pose = route_handler.getGoalPose(); if ( route_handler.isInGoalRouteSection(current_lanes.back()) && - lane_change_total_length + lane_change_required_length > + lane_change_length + minimum_lane_change_length_to_preferred_lane > utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { return false; } @@ -541,7 +302,7 @@ bool hasEnoughLength( } if ( - lane_change_total_length + lane_change_required_length > + lane_change_length + minimum_lane_change_length_to_preferred_lane > utils::getDistanceToEndOfLane(current_pose, target_lanes)) { return false; } @@ -549,22 +310,25 @@ bool hasEnoughLength( return true; } -bool isLaneChangePathSafe( +PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, const LaneChangeTargetObjectIndices & dynamic_objects_indices, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_parameter, const double front_decel, - const double rear_decel, Pose & ego_pose_before_collision, - std::unordered_map & debug_data, const double acceleration) + const double rear_decel, std::unordered_map & debug_data, + const double acceleration) { + PathSafetyStatus path_safety_status; + if (dynamic_objects == nullptr) { - return true; + return path_safety_status; } const auto & path = lane_change_path.path; if (path.points.empty()) { - return false; + path_safety_status.is_safe = false; + return path_safety_status; } const double time_resolution = lane_change_parameter.prediction_time_resolution; @@ -572,13 +336,13 @@ bool isLaneChangePathSafe( const double check_start_time = check_at_prepare_phase ? 0.0 : lane_change_path.duration.prepare; const double check_end_time = lane_change_path.duration.sum(); - const double & prepare_duration = lane_change_parameter.prepare_duration; + const double & prepare_duration = common_parameter.lane_change_prepare_duration; const auto current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, common_parameter.ego_nearest_dist_threshold, common_parameter.ego_nearest_yaw_threshold); - const auto vehicle_predicted_path = utils::convertToPredictedPath( + const auto ego_predicted_path = utils::convertToPredictedPath( path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution, prepare_duration, acceleration); const auto & vehicle_info = common_parameter.vehicle_info; @@ -622,7 +386,7 @@ bool isLaneChangePathSafe( for (double t = check_start_time; t < check_end_time; t += time_resolution) { tier4_autoware_utils::Polygon2d ego_polygon; const auto result = - utils::getEgoExpectedPoseAndConvertToPolygon(vehicle_predicted_path, t, vehicle_info); + utils::getEgoExpectedPoseAndConvertToPolygon(ego_predicted_path, t, vehicle_info); if (!result) { continue; } @@ -633,108 +397,70 @@ bool isLaneChangePathSafe( for (const auto & i : in_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); auto current_debug_data = assignDebugData(obj); - const auto predicted_paths = + const auto obj_predicted_paths = utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); - for (const auto & obj_path : predicted_paths) { + for (const auto & obj_path : obj_predicted_paths) { if (!utils::safety_check::isSafeInLaneletCollisionCheck( - interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, - obj, obj_path, common_parameter, + path, interpolated_ego, current_twist, check_durations, + lane_change_path.duration.prepare, obj, obj_path, common_parameter, lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, - rear_decel, ego_pose_before_collision, current_debug_data.second)) { + rear_decel, current_debug_data.second)) { + path_safety_status.is_safe = false; appendDebugInfo(current_debug_data, false); - return false; + if (isObjectIndexIncluded(i, dynamic_objects_indices.target_lane)) { + const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= + !utils::safety_check::isTargetObjectFront( + path, current_pose, common_parameter.vehicle_info, obj_polygon); + } } } appendDebugInfo(current_debug_data, true); } if (!lane_change_parameter.use_predicted_path_outside_lanelet) { - return true; + return path_safety_status; } for (const auto & i : dynamic_objects_indices.other_lane) { const auto & obj = dynamic_objects->objects.at(i); auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); + current_debug_data.second.ego_predicted_path.push_back(ego_predicted_path); const auto predicted_paths = utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); if (!utils::safety_check::isSafeInFreeSpaceCollisionCheck( - interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj, - common_parameter, lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, - front_decel, rear_decel, current_debug_data.second)) { + path, interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, + obj, common_parameter, + lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, + rear_decel, current_debug_data.second)) { + path_safety_status.is_safe = false; appendDebugInfo(current_debug_data, false); - return false; + const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( + path, current_pose, common_parameter.vehicle_info, obj_polygon); } appendDebugInfo(current_debug_data, true); } - return true; -} - -PathWithLaneId getPrepareSegment( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double prepare_velocity) -{ - if (original_lanelets.empty()) { - return PathWithLaneId(); - } - - const double s_start = arc_length_from_current - backward_path_length; - const double s_end = arc_length_from_current + prepare_length; - - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getPrepareSegment"), - "start: %f, end: %f", s_start, s_end); - - PathWithLaneId prepare_segment = - route_handler.getCenterLinePath(original_lanelets, s_start, s_end); - - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); - - return prepare_segment; + return path_safety_status; } -PathWithLaneId getPrepareSegment( - const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets, - const Pose & current_pose, const double backward_path_length, const double prepare_length, - const double prepare_velocity) +bool isObjectIndexIncluded( + const size_t & index, const std::vector & dynamic_objects_indices) { - if (original_lanelets.empty()) { - return PathWithLaneId(); - } - - auto prepare_segment = original_path; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, current_pose, 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); - - return prepare_segment; + return std::count(dynamic_objects_indices.begin(), dynamic_objects_indices.end(), index) != 0; } double calcLaneChangingLength( - const double lane_changing_velocity, const double shift_length, - const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param) + const double lane_changing_velocity, const double shift_length, const double lateral_acc, + const double lateral_jerk) { - const double lateral_acc = lane_changing_velocity < lc_param.lateral_acc_switching_velocity - ? lc_param.lane_changing_lateral_acc_at_low_velocity - : lc_param.lane_changing_lateral_acc; - const auto required_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lc_param.lane_changing_lateral_jerk, lateral_acc); - - const double & min_lane_change_length = com_param.minimum_lane_changing_length; - const double lane_changing_length = - std::max(lane_changing_velocity * required_time, min_lane_change_length); + const auto required_time = + PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); + const auto lane_changing_length = lane_changing_velocity * required_time; RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") @@ -791,7 +517,7 @@ PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route) + const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -801,7 +527,8 @@ PathWithLaneId getReferencePathFromTargetLane( const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; if (is_goal_in_route) { const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - + next_lane_change_buffer; return std::min(dist_from_lc_start, s_goal); } return std::min(dist_from_lc_start, target_lane_length); @@ -1032,220 +759,6 @@ std::vector generateDrivableLanes( return drivable_lanes; } -std::vector combineDrivableLanes( - const std::vector & original_drivable_lanes_vec, - const std::vector & new_drivable_lanes_vec) -{ - const auto has_same_lane = - [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { - return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { - return ll.id() == target_lane.id(); - }) != lanes.end(); - }; - - const auto convert_to_lanes = [](const DrivableLanes & drivable_lanes) { - auto lanes = drivable_lanes.middle_lanes; - lanes.push_back(drivable_lanes.right_lane); - lanes.push_back(drivable_lanes.left_lane); - return lanes; - }; - - auto updated_drivable_lanes_vec = original_drivable_lanes_vec; - size_t new_drivable_lanes_idx = 0; - for (auto & updated_drivable_lanes : updated_drivable_lanes_vec) { - // calculated corresponding index of new_drivable_lanes - const auto opt_new_drivable_lanes_idx = [&]() -> std::optional { - for (size_t n_idx = 0; n_idx < new_drivable_lanes_vec.size(); ++n_idx) { - for (const auto & ll : convert_to_lanes(updated_drivable_lanes)) { - if (has_same_lane(ll, convert_to_lanes(new_drivable_lanes_vec.at(n_idx)))) { - return n_idx; - } - } - } - return std::nullopt; - }(); - if (!opt_new_drivable_lanes_idx) { - continue; - } - new_drivable_lanes_idx = *opt_new_drivable_lanes_idx; - const auto & new_drivable_lanes = new_drivable_lanes_vec.at(new_drivable_lanes_idx); - - // update left lane - if (has_same_lane(updated_drivable_lanes.left_lane, convert_to_lanes(new_drivable_lanes))) { - updated_drivable_lanes.left_lane = new_drivable_lanes.left_lane; - } - // update right lane - if (has_same_lane(updated_drivable_lanes.right_lane, convert_to_lanes(new_drivable_lanes))) { - updated_drivable_lanes.right_lane = new_drivable_lanes.right_lane; - } - // update middle lanes - for (const auto & middle_lane : convert_to_lanes(new_drivable_lanes)) { - if (!has_same_lane(middle_lane, convert_to_lanes(updated_drivable_lanes))) { - updated_drivable_lanes.middle_lanes.push_back(middle_lane); - } - } - - // validate middle lanes - auto & middle_lanes = updated_drivable_lanes.middle_lanes; - if (has_same_lane(updated_drivable_lanes.right_lane, middle_lanes)) { - middle_lanes.erase( - std::remove( - std::begin(middle_lanes), std::end(middle_lanes), updated_drivable_lanes.right_lane), - std::cend(middle_lanes)); - } - if (has_same_lane(updated_drivable_lanes.left_lane, middle_lanes)) { - middle_lanes.erase( - std::remove( - std::begin(middle_lanes), std::end(middle_lanes), updated_drivable_lanes.left_lane), - std::cend(middle_lanes)); - } - } - // NOTE: If original_drivable_lanes_vec is shorter than new_drivable_lanes_vec, push back remained - // new_drivable_lanes_vec. - updated_drivable_lanes_vec.insert( - updated_drivable_lanes_vec.end(), new_drivable_lanes_vec.begin() + new_drivable_lanes_idx + 1, - new_drivable_lanes_vec.end()); - - return updated_drivable_lanes_vec; -} - -std::optional getAbortPaths( - const std::shared_ptr & planner_data, const LaneChangePath & selected_path, - [[maybe_unused]] const Pose & ego_pose_before_collision, - const BehaviorPathPlannerParameters & common_param, - [[maybe_unused]] const LaneChangeParameters & lane_change_param) -{ - const auto & route_handler = planner_data->route_handler; - const auto current_velocity = planner_data->self_odometry->twist.twist.linear.x; - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto reference_lanelets = selected_path.reference_lanelets; - - const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; - const auto ego_nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - const double minimum_lane_change_length = utils::calcLaneChangeBuffer(common_param, 1); - - const auto & lane_changing_path = selected_path.path; - const auto lane_changing_end_pose_idx = std::invoke([&]() { - constexpr double s_start = 0.0; - const double s_end = - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length; - - const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); - return motion_utils::findFirstNearestIndexWithSoftConstraints( - lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - }); - - const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - - const auto get_abort_idx_and_distance = [&](const double param_time) { - double turning_point_dist{0.0}; - if (ego_pose_idx > lane_changing_end_pose_idx) { - return std::make_pair(ego_pose_idx, turning_point_dist); - } - - constexpr auto min_velocity{2.77}; - const auto desired_distance = std::max(min_velocity, current_velocity) * param_time; - const auto & points = lane_changing_path.points; - size_t idx{0}; - for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { - const auto dist_to_ego = - utils::getSignedDistance(current_pose, points.at(idx).point.pose, reference_lanelets); - turning_point_dist = dist_to_ego; - if (dist_to_ego > desired_distance) { - break; - } - } - return std::make_pair(idx, turning_point_dist); - }; - - const auto abort_delta_time = lane_change_param.abort_delta_time; - const auto [abort_start_idx, abort_start_dist] = get_abort_idx_and_distance(abort_delta_time); - const auto [abort_return_idx, abort_return_dist] = - get_abort_idx_and_distance(abort_delta_time * 2); - - if (abort_start_idx >= abort_return_idx) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "abort start idx and return idx is equal. can't compute abort path."); - return std::nullopt; - } - - if (!hasEnoughLengthToLaneChangeAfterAbort( - *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "insufficient distance to abort."); - return std::nullopt; - } - - const auto abort_start_pose = lane_changing_path.points.at(abort_start_idx).point.pose; - const auto abort_return_pose = lane_changing_path.points.at(abort_return_idx).point.pose; - const auto arc_position = - lanelet::utils::getArcCoordinates(reference_lanelets, abort_return_pose); - const PathWithLaneId reference_lane_segment = std::invoke([&]() { - const double minimum_lane_changing_length = - common_param.backward_length_buffer_for_end_of_lane + - common_param.minimum_lane_changing_length; - - const double s_start = arc_position.length; - double s_end = - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_changing_length; - - if (route_handler->isInGoalRouteSection(selected_path.target_lanelets.back())) { - const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates( - selected_path.target_lanelets, route_handler->getGoalPose()); - s_end = std::min(s_end, goal_arc_coordinates.length) - minimum_lane_changing_length; - } - PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); - ref.points.back().point.longitudinal_velocity_mps = std::min( - ref.points.back().point.longitudinal_velocity_mps, - static_cast(lane_change_param.minimum_lane_changing_velocity)); - return ref; - }); - - ShiftLine shift_line; - shift_line.start = abort_start_pose; - shift_line.end = abort_return_pose; - shift_line.end_shift_length = -arc_position.distance; - shift_line.start_idx = abort_start_idx; - shift_line.end_idx = abort_return_idx; - - PathShifter path_shifter; - path_shifter.setPath(lane_changing_path); - path_shifter.addShiftLine(shift_line); - const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( - shift_line.end_shift_length, abort_start_dist, current_velocity); - - if (lateral_jerk > lane_change_param.abort_max_lateral_jerk) { - return std::nullopt; - } - - ShiftedPath shifted_path; - // offset front side - // bool offset_back = false; - if (!path_shifter.generate(&shifted_path)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "failed to generate abort shifted path."); - } - - PathWithLaneId start_to_abort_return_pose; - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), reference_lane_segment.points.begin(), - reference_lane_segment.points.end()); - - auto abort_path = selected_path; - abort_path.shifted_path = shifted_path; - abort_path.path = start_to_abort_return_pose; - abort_path.shift_line = shift_line; - return std::optional{abort_path}; -} - double getLateralShift(const LaneChangePath & path) { const auto start_idx = path.shift_line.start_idx; @@ -1257,11 +770,12 @@ double getLateralShift(const LaneChangePath & path) bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param) + const BehaviorPathPlannerParameters & common_param, const Direction direction) { - const auto minimum_lane_change_length = common_param.minimum_prepare_length + - common_param.minimum_lane_changing_length + - common_param.backward_length_buffer_for_end_of_lane; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); + const double minimum_lane_change_length = + utils::calcMinimumLaneChangeLength(common_param, shift_intervals); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1278,18 +792,23 @@ bool hasEnoughLengthToLaneChangeAfterAbort( } // TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane -lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( - const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lane, +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length) { - const auto arc_length = lanelet::utils::getArcCoordinates({target_lane}, current_pose); + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); if (arc_length.length >= backward_length) { return {}; } + const auto & front_lane = target_lanes.front(); const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( - target_lane, std::abs(backward_length - arc_length.length), {target_lane}); + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); lanelet::ConstLanelets backward_lanes{}; const auto num_of_lanes = std::invoke([&preceding_lanes]() { @@ -1421,4 +940,88 @@ std::string getStrDirection(const std::string & name, const Direction direction) return ""; } +std::vector> getSortedLaneIds( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) +{ + std::vector> sorted_lane_ids{}; + sorted_lane_ids.reserve(target_lanes.size()); + const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { + const auto routing_graph_ptr = route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelet lane; + if (rough_shift_length < 0.0) { + // lane change to the left, so i wan to take the lane right to target + const auto has_target_right = routing_graph_ptr->right(target_lane); + if (has_target_right) { + lane = *has_target_right; + } + } else if (rough_shift_length > 0.0) { + const auto has_target_left = routing_graph_ptr->left(target_lane); + if (has_target_left) { + lane = *has_target_left; + } + } else { + lane = target_lane; + } + + const auto find_same_id = std::find_if( + current_lanes.cbegin(), current_lanes.cend(), + [&lane](const lanelet::ConstLanelet & orig) { return orig.id() == lane.id(); }); + + if (find_same_id == current_lanes.cend()) { + return std::vector{target_lane.id()}; + } + + if (target_lane.id() > find_same_id->id()) { + return std::vector{find_same_id->id(), target_lane.id()}; + } + + return std::vector{target_lane.id(), find_same_id->id()}; + }; + + std::transform( + target_lanes.cbegin(), target_lanes.cend(), std::back_inserter(sorted_lane_ids), + get_sorted_lane_ids); + + return sorted_lane_ids; +} + +std::vector replaceWithSortedIds( + const std::vector & original_lane_ids, + const std::vector> & sorted_lane_ids) +{ + for (const auto original_id : original_lane_ids) { + for (const auto & sorted_id : sorted_lane_ids) { + if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { + return sorted_id; + } + } + } + return original_lane_ids; +} + +CandidateOutput assignToCandidate( + const LaneChangePath & lane_change_path, const Point & ego_position) +{ + CandidateOutput candidate_output; + candidate_output.path_candidate = lane_change_path.path; + candidate_output.lateral_shift = utils::lane_change::getLateralShift(lane_change_path); + candidate_output.start_distance_to_path_change = motion_utils::calcSignedArcLength( + lane_change_path.path.points, ego_position, lane_change_path.shift_line.start.position); + candidate_output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( + lane_change_path.path.points, ego_position, lane_change_path.shift_line.end.position); + + return candidate_output; +} + +boost::optional getLaneChangeTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType type, const Direction & direction) +{ + if (type == LaneChangeModuleType::NORMAL) { + return route_handler.getLaneChangeTarget(current_lanes, direction); + } + + return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index c6356b8ca42c2..7a9e7ceb5a125 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -69,9 +69,14 @@ void PathShifter::setVelocity(const double velocity) velocity_ = velocity; } -void PathShifter::setLateralAccelerationLimit(const double acc) +void PathShifter::setLateralAccelerationLimit(const double lateral_acc) { - acc_limit_ = acc; + lateral_acc_limit_ = lateral_acc; +} + +void PathShifter::setLongitudinalAcceleration(const double longitudinal_acc) +{ + longitudinal_acc_ = longitudinal_acc; } void PathShifter::addShiftLine(const ShiftLine & line) @@ -271,60 +276,103 @@ std::pair, std::vector> PathShifter::getBaseLengthsW return std::pair{base_lon, base_lat}; } +std::pair, std::vector> PathShifter::getBaseLengthsWithoutAccelLimit( + const double arclength, const double shift_length, const double velocity, + const double longitudinal_acc, const double total_time, const bool offset_back) const +{ + const auto & s = arclength; + const auto & l = shift_length; + const auto & v0 = velocity; + const auto & a = longitudinal_acc; + const auto & T = total_time; + const double t = T / 4; + + const double s1 = v0 * t + 0.5 * a * t * t; + const double v1 = v0 + a * t; + const double s2 = s1 + 2 * v1 * t + 2 * a * t * t; + std::vector base_lon = {0.0, s1, s2, s}; + std::vector base_lat = {0.0, 1.0 / 12.0 * l, 11.0 / 12.0 * l, l}; + + if (!offset_back) std::reverse(base_lat.begin(), base_lat.end()); + + return std::pair{base_lon, base_lat}; +} + std::pair, std::vector> PathShifter::calcBaseLengths( const double arclength, const double shift_length, const bool offset_back) const { - const auto speed = std::abs(velocity_); + const auto v0 = std::abs(velocity_); + + // For longitudinal acceleration, we only consider positive side + // negative acceleration (deceleration) is treated as 0.0 + const double acc_threshold = 0.0001; + const auto & a = longitudinal_acc_ > acc_threshold ? longitudinal_acc_ : 0.0; - if (speed < 1.0e-5) { + if (v0 < 1.0e-5 && a < acc_threshold) { // no need to consider acceleration limit - RCLCPP_DEBUG(logger_, "set velocity is zero. acc limit is ignored"); + RCLCPP_DEBUG(logger_, "set velocity is zero. lateral acc limit is ignored"); return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); } - const auto T = arclength / speed; + const auto & S = arclength; const auto L = std::abs(shift_length); - const auto a_max = 8.0 * L / (T * T); + const auto T = a > acc_threshold ? (-v0 + std::sqrt(v0 * v0 + 2 * a * S)) / a : S / v0; + const auto lateral_a_max = 8.0 * L / (T * T); - if (a_max < acc_limit_) { + if (lateral_a_max < lateral_acc_limit_) { // no need to consider acceleration limit RCLCPP_WARN_THROTTLE( - logger_, clock_, 3000, "No need to consider acc limit. max: %f, limit: %f", a_max, - acc_limit_); - return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + logger_, clock_, 3000, "No need to consider lateral acc limit. max: %f, limit: %f", + lateral_a_max, lateral_acc_limit_); + return getBaseLengthsWithoutAccelLimit(S, shift_length, v0, a, T, offset_back); } - const auto tj = T / 2.0 - 2.0 * L / (acc_limit_ * T); - const auto ta = 4.0 * L / (acc_limit_ * T) - T / 2.0; - const auto jerk = (2.0 * acc_limit_ * acc_limit_ * T) / (acc_limit_ * T * T - 4.0 * L); + const auto tj = T / 2.0 - 2.0 * L / (lateral_acc_limit_ * T); + const auto ta = 4.0 * L / (lateral_acc_limit_ * T) - T / 2.0; + const auto lat_jerk = + (2.0 * lateral_acc_limit_ * lateral_acc_limit_ * T) / (lateral_acc_limit_ * T * T - 4.0 * L); - if (tj < 0.0 || ta < 0.0 || jerk < 0.0 || tj / T < 0.1) { + if (tj < 0.0 || ta < 0.0 || lat_jerk < 0.0 || tj / T < 0.1) { // no need to consider acceleration limit RCLCPP_WARN_THROTTLE( logger_, clock_, 3000, "Acc limit is too small to be applied. Tj: %f, Ta: %f, j: %f, a_max: %f, acc_limit: %f", tj, - ta, jerk, a_max, acc_limit_); - return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + ta, lat_jerk, lateral_a_max, lateral_acc_limit_); + return getBaseLengthsWithoutAccelLimit(S, shift_length, offset_back); } const auto tj3 = tj * tj * tj; const auto ta2_tj = ta * ta * tj; const auto ta_tj2 = ta * tj * tj; - const auto s1 = tj * speed; - const auto s2 = s1 + ta * speed; - const auto s3 = s2 + tj * speed; // = s4 - const auto s5 = s3 + tj * speed; - const auto s6 = s5 + ta * speed; - const auto s7 = s6 + tj * speed; + const auto s1 = tj * v0 + 0.5 * a * tj * tj; + const auto v1 = v0 + a * tj; + + const auto s2 = s1 + ta * v1 + 0.5 * a * ta * ta; + const auto v2 = v1 + a * ta; + + const auto s3 = s2 + tj * v2 + 0.5 * a * tj * tj; // = s4 + const auto v3 = v2 + a * tj; + + const auto s5 = s3 + tj * v3 + 0.5 * a * tj * tj; + const auto v5 = v3 + a * tj; + + const auto s6 = s5 + ta * v5 + 0.5 * a * ta * ta; + const auto v6 = v5 + a * ta; + + const auto s7 = s6 + tj * v6 + 0.5 * a * tj * tj; const auto sign = shift_length > 0.0 ? 1.0 : -1.0; - const auto l1 = sign * (1.0 / 6.0 * jerk * tj3); - const auto l2 = sign * (1.0 / 6.0 * jerk * tj3 + 0.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); - const auto l3 = sign * (jerk * tj3 + 1.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); // = l4 - const auto l5 = sign * (11.0 / 6.0 * jerk * tj3 + 2.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); - const auto l6 = sign * (11.0 / 6.0 * jerk * tj3 + 3.0 * jerk * ta_tj2 + jerk * ta2_tj); - const auto l7 = sign * (2.0 * jerk * tj3 + 3.0 * jerk * ta_tj2 + jerk * ta2_tj); + const auto l1 = sign * (1.0 / 6.0 * lat_jerk * tj3); + const auto l2 = + sign * (1.0 / 6.0 * lat_jerk * tj3 + 0.5 * lat_jerk * ta_tj2 + 0.5 * lat_jerk * ta2_tj); + const auto l3 = + sign * (lat_jerk * tj3 + 1.5 * lat_jerk * ta_tj2 + 0.5 * lat_jerk * ta2_tj); // = l4 + const auto l5 = + sign * (11.0 / 6.0 * lat_jerk * tj3 + 2.5 * lat_jerk * ta_tj2 + 0.5 * lat_jerk * ta2_tj); + const auto l6 = + sign * (11.0 / 6.0 * lat_jerk * tj3 + 3.0 * lat_jerk * ta_tj2 + lat_jerk * ta2_tj); + const auto l7 = sign * (2.0 * lat_jerk * tj3 + 3.0 * lat_jerk * ta_tj2 + lat_jerk * ta2_tj); std::vector base_lon = {0.0, s1, s2, s3, s5, s6, s7}; std::vector base_lat = {0.0, l1, l2, l3, l5, l6, l7}; diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index e168e8bd9fcf8..5a5c1135d4c58 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -101,16 +101,14 @@ PathWithLaneId resamplePathWithSpline( const auto start_s = std::max(target_section.first, 0.0); const auto end_s = std::min(target_section.second, s_vec.back()); - for (double s = start_s; s < end_s; s += interval) { + for (double s = start_s; s < end_s - epsilon; s += interval) { if (!has_almost_same_value(s_out, s)) { s_out.push_back(s); } } // Insert Terminal Point - if (!has_almost_same_value(s_out, end_s)) { - s_out.push_back(end_s); - } + s_out.push_back(end_s); // Insert Stop Point const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path); @@ -469,4 +467,58 @@ std::vector interpolatePose( return interpolated_poses; } +Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) +{ + if (prev_path.path.points.empty()) { + return ego_pose; + } + + // un-shifted for current ideal pose + const auto closest_idx = motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); + + // NOTE: Considering avoidance by motion, we set unshifted_pose as previous path instead of + // ego_pose. + auto unshifted_pose = motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; + + unshifted_pose = tier4_autoware_utils::calcOffsetPose( + unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); + unshifted_pose.orientation = ego_pose.orientation; + + return unshifted_pose; +} + +// TODO(Horibe) clean up functions: there is a similar code in util as well. +PathWithLaneId calcCenterLinePath( + const std::shared_ptr & planner_data, const Pose & ref_pose, + const double longest_dist_to_shift_line, const std::optional & prev_module_path) +{ + const auto & p = planner_data->parameters; + const auto & route_handler = planner_data->route_handler; + + PathWithLaneId centerline_path; + + const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. + const auto backward_length = + std::max(p.backward_path_length, longest_dist_to_shift_line + extra_margin); + + RCLCPP_DEBUG( + rclcpp::get_logger("path_utils"), + "p.backward_path_length = %f, longest_dist_to_shift_line = %f, backward_length = %f", + p.backward_path_length, longest_dist_to_shift_line, backward_length); + + const lanelet::ConstLanelets current_lanes = [&]() { + if (!prev_module_path) { + return utils::calcLaneAroundPose( + route_handler, ref_pose, p.forward_path_length, backward_length); + } + return utils::getCurrentLanesFromPath(*prev_module_path, planner_data); + }(); + + centerline_path = utils::getCenterLinePath( + *route_handler, current_lanes, ref_pose, backward_length, p.forward_path_length, p); + + centerline_path.header = route_handler->getRouteHeader(); + + return centerline_path; +} } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp index 4dbe7bc2d890f..21111f5b06ee6 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp @@ -28,11 +28,11 @@ namespace behavior_path_planner using pull_out_utils::combineReferencePath; using pull_out_utils::getPullOutLanes; -GeometricPullOut::GeometricPullOut( - rclcpp::Node & node, const PullOutParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters) -: PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parallel_parking_parameters} +GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const PullOutParameters & parameters) +: PullOutPlannerBase{node, parameters}, + parallel_parking_parameters_{parameters.parallel_parking_parameters} { + planner_.setParameters(parallel_parking_parameters_); } boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) @@ -45,8 +45,9 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p auto lanes = road_lanes; lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - // todo: set params only once? - planner_.setData(planner_data_, parallel_parking_parameters_); + planner_.setTurningRadius( + planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); + planner_.setPlannerData(planner_data_); const bool found_valid_path = planner_.planPullOut(start_pose, goal_pose, road_lanes, shoulder_lanes); if (!found_valid_path) { @@ -68,7 +69,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p auto partial_paths = planner_.getPaths(); // remove stop velocity of first arc path partial_paths.front().points.back().point.longitudinal_velocity_mps = - parameters_.geometric_pull_out_velocity; + parallel_parking_parameters_.pull_out_velocity; const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); } diff --git a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp index 319cfeacd76ad..0a8866a6978ef 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp @@ -92,9 +92,10 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check lane departure const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); if (lane_departure_checker_->checkPathWillLeaveLane( utils::transformToLanelets(expanded_lanes), path_start_to_end)) { continue; @@ -108,9 +109,11 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } // Generate drivable area + // for old architecture + // NOTE: drivable_area_info is assigned outside this function. const auto shorten_lanes = utils::cutOverlappedLanes(shift_path, drivable_lanes); utils::generateDrivableArea( - shift_path, shorten_lanes, common_parameters.vehicle_length, planner_data_); + shift_path, shorten_lanes, false, common_parameters.vehicle_length, planner_data_); shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index 0bd83be6d059b..cc5d698813df5 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -14,205 +14,175 @@ #include "behavior_path_planner/utils/safety_check.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "perception_utils/predicted_path_utils.hpp" namespace behavior_path_planner::utils::safety_check { -template -ProjectedDistancePoint pointToSegment( - const Point2d & reference_point, const Point2d & polygon_segment_start, - const Point2d & polygon_segment_end) +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { - auto copied_point_from_object = polygon_segment_end; - auto copied_point_from_reference = reference_point; - bg::subtract_point(copied_point_from_object, polygon_segment_start); - bg::subtract_point(copied_point_from_reference, polygon_segment_start); - - const auto c1 = bg::dot_product(copied_point_from_reference, copied_point_from_object); - if (!(c1 > 0)) { - return {polygon_segment_start, Pythagoras::apply(reference_point, polygon_segment_start)}; - } - - const auto c2 = bg::dot_product(copied_point_from_object, copied_point_from_object); - if (!(c2 > c1)) { - return {polygon_segment_end, Pythagoras::apply(reference_point, polygon_segment_end)}; - } + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; - Point2d projected = polygon_segment_start; - bg::multiply_value(copied_point_from_object, c1 / c2); - bg::add_point(projected, copied_point_from_object); - - return {projected, Pythagoras::apply(reference_point, projected)}; + bg::append(polygon.outer(), point); } -void getProjectedDistancePointFromPolygons( - const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, - Pose & point_on_object) +bool isTargetObjectFront( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) { - ProjectedDistancePoint nearest; - std::unique_ptr current_point; - - bool points_in_ego{false}; - - const auto findPoints = [&nearest, ¤t_point, &points_in_ego]( - const Polygon2d & polygon_for_segment, - const Polygon2d & polygon_for_points, const bool & ego_is_points) { - const auto segments = boost::make_iterator_range( - bg::segments_begin(polygon_for_segment), bg::segments_end(polygon_for_segment)); - const auto points = boost::make_iterator_range( - bg::points_begin(polygon_for_points), bg::points_end(polygon_for_points)); - - for (auto && segment : segments) { - for (auto && point : points) { - const auto projected = pointToSegment(point, *segment.first, *segment.second); - if (!current_point || projected.distance < nearest.distance) { - current_point = std::make_unique(point); - nearest = projected; - points_in_ego = ego_is_points; - } - } + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { + return true; } - }; - - std::invoke(findPoints, ego_polygon, object_polygon, false); - std::invoke(findPoints, object_polygon, ego_polygon, true); - - if (!points_in_ego) { - point_on_object.position.x = current_point->x(); - point_on_object.position.y = current_point->y(); - point_on_ego.position.x = nearest.projected_point.x(); - point_on_ego.position.y = nearest.projected_point.y(); - } else { - point_on_ego.position.x = current_point->x(); - point_on_ego.position.y = current_point->y(); - point_on_object.position.x = nearest.projected_point.x(); - point_on_object.position.y = nearest.projected_point.y(); } -} -Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object) -{ - tf2::Transform tf_map_desired_to_global{}; - tf2::Transform tf_map_target_to_global{}; - - tf2::fromMsg(desired_object, tf_map_desired_to_global); - tf2::fromMsg(target_object, tf_map_target_to_global); - Pose desired_obj_pose_projected_to_target{}; - tf2::toMsg( - tf_map_desired_to_global.inverse() * tf_map_target_to_global, - desired_obj_pose_projected_to_target); - - return desired_obj_pose_projected_to_target; + return false; } -bool hasEnoughDistance( - const Pose & expected_ego_pose, const Twist & ego_current_twist, - const Pose & expected_object_pose, const Twist & object_current_twist, - const BehaviorPathPlannerParameters & param, const double front_decel, const double rear_decel, - CollisionCheckDebug & debug) +Polygon2d createExtendedPolygon( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const double lon_length, const double lat_margin) { - const auto front_vehicle_pose = - projectCurrentPoseToTarget(expected_ego_pose, expected_object_pose); - debug.relative_to_ego = front_vehicle_pose; + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + const double lon_offset = std::max(lon_length + base_to_front, base_to_front); + const double lat_offset = width / 2.0 + lat_margin; + const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); + const auto p2 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0); + const auto p3 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0); + const auto p4 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0); + + Polygon2d polygon; + appendPointToPolygon(polygon, p1.position); + appendPointToPolygon(polygon, p2.position); + appendPointToPolygon(polygon, p3.position); + appendPointToPolygon(polygon, p4.position); + appendPointToPolygon(polygon, p1.position); + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} - // 1. Check lateral distance between ego and target object - if (std::abs(front_vehicle_pose.position.y) > param.lateral_distance_max_threshold) { - return true; +Polygon2d createExtendedPolygon( + const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin) +{ + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); + if (obj_polygon.outer().empty()) { + return obj_polygon; } - const auto is_obj_in_front = front_vehicle_pose.position.x > -1e-3; - debug.is_front = is_obj_in_front; - - const auto [front_vehicle_velocity, rear_vehicle_velocity] = std::invoke([&]() { - debug.object_twist.linear = object_current_twist.linear; - if (is_obj_in_front) { - return std::make_pair(object_current_twist.linear.x, ego_current_twist.linear.x); - } - return std::make_pair(ego_current_twist.linear.x, object_current_twist.linear.x); - }); - - // 2. Check physical distance between ego and target object - const auto is_unsafe_dist_between_vehicle = std::invoke([&]() { - // ignore this for parked vehicle. - if (object_current_twist.linear.x < 0.1) { - return false; - } - - // the value guarantee distance between vehicles are always more than dist - const auto max_vel = std::max(front_vehicle_velocity, rear_vehicle_velocity); - constexpr auto scale = 0.8; - const auto dist = scale * std::abs(max_vel) + param.longitudinal_distance_min_threshold; - - // return value rounded to the nearest two floating point - return std::abs(front_vehicle_pose.position.x) < dist; - }); - - if (is_unsafe_dist_between_vehicle) { - return false; + double max_x = std::numeric_limits::lowest(); + double min_x = std::numeric_limits::max(); + double max_y = std::numeric_limits::lowest(); + double min_y = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose); + + max_x = std::max(transformed_p.x, max_x); + min_x = std::min(transformed_p.x, min_x); + max_y = std::max(transformed_p.y, max_y); + min_y = std::min(transformed_p.y, min_y); } - // 3. Check longitudinal distance after deceleration + const double lon_offset = max_x + lon_length; + const double left_lat_offset = max_y + lat_margin; + const double right_lat_offset = min_y - lat_margin; + const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); + const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0); + const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0); + const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0); + + Polygon2d polygon; + appendPointToPolygon(polygon, p1.position); + appendPointToPolygon(polygon, p2.position); + appendPointToPolygon(polygon, p3.position); + appendPointToPolygon(polygon, p4.position); + appendPointToPolygon(polygon, p1.position); + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + +double calcRssDistance( + const double front_object_velocity, const double rear_object_velocity, + const double front_object_deceleration, const double rear_object_deceleration, + const BehaviorPathPlannerParameters & params) +{ const auto stoppingDistance = [](const auto vehicle_velocity, const auto vehicle_accel) { // compensate if user accidentally set the deceleration to some positive value const auto deceleration = (vehicle_accel < -1e-3) ? vehicle_accel : -1.0; return -std::pow(vehicle_velocity, 2) / (2.0 * deceleration); }; - const auto front_vehicle_stop_threshold = - stoppingDistance(front_vehicle_velocity, front_decel) + std::abs(front_vehicle_pose.position.x); + const double & reaction_time = + params.rear_vehicle_reaction_time + params.rear_vehicle_safety_time_margin; - // longitudinal_distance_min_threshold here guarantee future stopping distance must be more than - // longitudinal_distance_min_threshold - const auto rear_vehicle_stop_threshold = std::invoke([&]() { - const auto reaction_buffer = rear_vehicle_velocity * param.rear_vehicle_reaction_time; - const auto safety_buffer = rear_vehicle_velocity * param.rear_vehicle_safety_time_margin; - return std::max( - reaction_buffer + safety_buffer + stoppingDistance(rear_vehicle_velocity, rear_decel), - param.longitudinal_distance_min_threshold); - }); + const double front_object_stop_length = + stoppingDistance(front_object_velocity, front_object_deceleration); + const double rear_object_stop_length = + rear_object_velocity * reaction_time + + stoppingDistance(rear_object_velocity, rear_object_deceleration); + return rear_object_stop_length - front_object_stop_length; +} - return rear_vehicle_stop_threshold <= front_vehicle_stop_threshold; +double calcMinimumLongitudinalLength( + const double front_object_velocity, const double rear_object_velocity, + const BehaviorPathPlannerParameters & params) +{ + const double & lon_threshold = params.longitudinal_distance_min_threshold; + const auto max_vel = std::max(front_object_velocity, rear_object_velocity); + constexpr auto scale = 0.8; + return scale * std::abs(max_vel) + lon_threshold; } bool isSafeInLaneletCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug) + const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug) { debug.lerped_path.reserve(check_duration.size()); - const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; - const auto object_speed = object_twist.linear.x; - const auto ignore_check_at_time = [&](const double current_time) { - return ( - (current_time < prepare_duration) && - (object_speed < prepare_phase_ignore_target_speed_thresh)); - }; + const auto & ego_velocity = ego_current_twist.linear.x; + const auto & object_velocity = + target_object.kinematics.initial_twist_with_covariance.twist.linear.x; for (size_t i = 0; i < check_duration.size(); ++i) { const auto current_time = check_duration.at(i); - if (ignore_check_at_time(current_time)) { + if ( + current_time < prepare_duration && + object_velocity < prepare_phase_ignore_target_velocity_thresh) { continue; } - const auto found_expected_obj_pose = - perception_utils::calcInterpolatedPose(target_object_path, current_time); - - if (!found_expected_obj_pose) { + const auto obj_pose = perception_utils::calcInterpolatedPose(target_object_path, current_time); + if (!obj_pose) { continue; } - auto expected_obj_pose = *found_expected_obj_pose; - const auto & obj_polygon = - tier4_autoware_utils::toPolygon2d(expected_obj_pose, target_object.shape); - const auto & ego_info = interpolated_ego.at(i); - auto expected_ego_pose = ego_info.first; - const auto & ego_polygon = ego_info.second; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, target_object.shape); + const auto & ego_pose = interpolated_ego.at(i).first; + const auto & ego_polygon = interpolated_ego.at(i).second; + // check overlap debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { @@ -220,49 +190,72 @@ bool isSafeInLaneletCollisionCheck( return false; } - debug.lerped_path.push_back(expected_ego_pose); - - getProjectedDistancePointFromPolygons( - ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); - debug.expected_ego_pose = expected_ego_pose; - debug.expected_obj_pose = expected_obj_pose; - - if (!hasEnoughDistance( - expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters, - front_decel, rear_decel, debug)) { - debug.failed_reason = "not_enough_longitudinal"; + // compute which one is at the front of the other + const bool is_object_front = + isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); + const auto & [front_object_velocity, rear_object_velocity] = + is_object_front ? std::make_pair(object_velocity, ego_velocity) + : std::make_pair(ego_velocity, object_velocity); + + // compute rss dist + const auto rss_dist = calcRssDistance( + front_object_velocity, rear_object_velocity, front_object_deceleration, + rear_object_deceleration, common_parameters); + + // minimum longitudinal length + const auto min_lon_length = + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + + const auto & lon_offset = std::max(rss_dist, min_lon_length); + const auto & ego_vehicle_info = common_parameters.vehicle_info; + const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & extended_ego_polygon = + is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin) + : ego_polygon; + const auto & extended_obj_polygon = + is_object_front + ? obj_polygon + : createExtendedPolygon(*obj_pose, target_object.shape, lon_offset, lat_margin); + + debug.lerped_path.push_back(ego_pose); + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = *obj_pose; + debug.is_front = is_object_front; + + if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { + debug.failed_reason = "overlap_extended_polygon"; return false; } - ego_pose_before_collision = expected_ego_pose; } return true; } bool isSafeInFreeSpaceCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug) + const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug) { + const auto & obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(target_object); - const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; - const auto object_speed = object_twist.linear.x; - const auto ignore_check_at_time = [&](const double current_time) { - return ( - (current_time < prepare_duration) && - (object_speed < prepare_phase_ignore_target_speed_thresh)); - }; + const auto & object_velocity = + target_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto & ego_velocity = ego_current_twist.linear.x; + for (size_t i = 0; i < check_duration.size(); ++i) { const auto current_time = check_duration.at(i); - if (ignore_check_at_time(current_time)) { + if ( + current_time < prepare_duration && + object_velocity < prepare_phase_ignore_target_velocity_thresh) { continue; } - const auto & ego_info = interpolated_ego.at(i); - auto expected_ego_pose = ego_info.first; - const auto & ego_polygon = ego_info.second; + + const auto & ego_pose = interpolated_ego.at(i).first; + const auto & ego_polygon = interpolated_ego.at(i).second; debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; @@ -271,19 +264,42 @@ bool isSafeInFreeSpaceCollisionCheck( return false; } - auto expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; - getProjectedDistancePointFromPolygons( - ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); - - debug.expected_ego_pose = expected_ego_pose; - debug.expected_obj_pose = expected_obj_pose; - - const auto object_twist = target_object.kinematics.initial_twist_with_covariance.twist; - if (!hasEnoughDistance( - expected_ego_pose, ego_current_twist, - target_object.kinematics.initial_pose_with_covariance.pose, object_twist, - common_parameters, front_decel, rear_decel, debug)) { - debug.failed_reason = "not_enough_longitudinal"; + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + + // compute which one is at the front of the other + const bool is_object_front = + isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); + const auto & [front_object_velocity, rear_object_velocity] = + is_object_front ? std::make_pair(object_velocity, ego_velocity) + : std::make_pair(ego_velocity, object_velocity); + // compute rss dist + const auto rss_dist = calcRssDistance( + front_object_velocity, rear_object_velocity, front_object_deceleration, + rear_object_deceleration, common_parameters); + + // minimum longitudinal length + const auto min_lon_length = + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + + const auto & lon_offset = std::max(rss_dist, min_lon_length); + const auto & ego_vehicle_info = common_parameters.vehicle_info; + const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & extended_ego_polygon = + is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin) + : ego_polygon; + const auto & extended_obj_polygon = + is_object_front + ? obj_polygon + : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin); + + debug.lerped_path.push_back(ego_pose); + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.is_front = is_object_front; + + if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { + debug.failed_reason = "overlap_extended_polygon"; return false; } } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index f09c877626d20..2c175b9df0d8c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -123,6 +123,405 @@ using geometry_msgs::msg::PoseWithCovarianceStamped; using tf2::fromMsg; using tier4_autoware_utils::Point2d; +namespace drivable_area_processing +{ +boost::optional intersect( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + // calculate intersection point + const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); + if (det == 0.0) { + return {}; + } + + const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; + const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; + if (t < 0 || 1 < t || s < 0 || 1 < s) { + return {}; + } + + geometry_msgs::msg::Point intersect_point; + intersect_point.x = t * p1.x + (1.0 - t) * p2.x; + intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + intersect_point.z = t * p1.z + (1.0 - t) * p2.z; + return intersect_point; +} + +boost::optional> intersectBound( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const std::vector & bound, const size_t seg_idx1, + const size_t seg_idx2) +{ + const size_t start_idx = + static_cast(std::max(0, static_cast(std::min(seg_idx1, seg_idx2)) - 5)); + const size_t end_idx = static_cast(std::min( + static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); + for (int i = start_idx; i < static_cast(end_idx); ++i) { + const auto intersect_point = intersect(p1, p2, bound.at(i), bound.at(i + 1)); + if (intersect_point) { + std::pair result; + result.first = static_cast(i); + result.second = intersect_point.get(); + return result; + } + } + return boost::none; +} + +double calcDistanceFromPointToSegment( + const geometry_msgs::msg::Point & segment_start_point, + const geometry_msgs::msg::Point & segment_end_point, + const geometry_msgs::msg::Point & target_point) +{ + const auto & a = segment_start_point; + const auto & b = segment_end_point; + const auto & p = target_point; + + const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); + const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + if (0 <= dot_val && dot_val <= squared_segment_length) { + const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); + const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); + return numerator / denominator; + } + + // target_point is outside the segment. + return std::min( + tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); +} + +PolygonPoint transformBoundFrenetCoordinate( + const std::vector & bound_points, + const geometry_msgs::msg::Point & target_point) +{ + // NOTE: findNearestSegmentIndex cannot be used since a bound's interval is sometimes too large to + // find wrong nearest index. + std::vector dist_to_bound_segment_vec; + for (size_t i = 0; i < bound_points.size() - 1; ++i) { + const double dist_to_bound_segment = + calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + dist_to_bound_segment_vec.push_back(dist_to_bound_segment); + } + + const size_t min_dist_seg_idx = std::distance( + dist_to_bound_segment_vec.begin(), + std::min_element(dist_to_bound_segment_vec.begin(), dist_to_bound_segment_vec.end())); + const double lon_dist_to_segment = + motion_utils::calcLongitudinalOffsetToSegment(bound_points, min_dist_seg_idx, target_point); + const double lat_dist_to_segment = + motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); + return PolygonPoint{target_point, min_dist_seg_idx, lon_dist_to_segment, lat_dist_to_segment}; +} + +std::vector generatePolygonInsideBounds( + const std::vector & bound, const std::vector & edge_points, + const bool is_object_right) +{ + constexpr double invalid_lat_dist_to_bound = 10.0; + + std::vector full_polygon; + for (const auto & edge_point : edge_points) { + const auto polygon_point = transformBoundFrenetCoordinate(bound, edge_point); + + // check lat dist for U-turn roads. + if ( + (is_object_right && invalid_lat_dist_to_bound < polygon_point.lat_dist_to_bound) || + (!is_object_right && polygon_point.lat_dist_to_bound < -invalid_lat_dist_to_bound)) { + return {}; + } + full_polygon.push_back(polygon_point); + } + + // 1. check the case where the polygon intersects the bound + std::vector inside_poly; + bool has_intersection = false; // NOTE: between obstacle polygon and bound + for (int i = 0; i < static_cast(full_polygon.size()); ++i) { + const auto & curr_poly = full_polygon.at(i); + const auto & prev_poly = full_polygon.at(i == 0 ? full_polygon.size() - 1 : i - 1); + + const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); + const bool is_prev_outside = prev_poly.is_outside_bounds(is_object_right); + + if (is_curr_outside && is_prev_outside) { + continue; + } + if (!is_curr_outside && !is_prev_outside) { + inside_poly.push_back(curr_poly); + continue; + } + + const auto intersection = intersectBound( + prev_poly.point, curr_poly.point, bound, prev_poly.bound_seg_idx, curr_poly.bound_seg_idx); + if (!intersection) { + continue; + } + const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment( + bound, intersection->first, intersection->second); + const auto intersect_point = + PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0}; + has_intersection = true; + + if (is_prev_outside && !is_curr_outside) { + inside_poly.push_back(intersect_point); + inside_poly.push_back(curr_poly); + continue; + } + // Here is if (!is_prev_outside && is_curr_outside). + inside_poly.push_back(prev_poly); + inside_poly.push_back(intersect_point); + continue; + } + if (has_intersection) { + return inside_poly; + } + + // 2. check the case where the polygon does not intersect the bound + const bool is_polygon_fully_inside_bounds = [&]() { + for (const auto & curr_poly : full_polygon) { + const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); + if (is_curr_outside) { + return false; + } + } + return true; + }(); + if (is_polygon_fully_inside_bounds) { + return full_polygon; + } + + return std::vector{}; +} + +std::vector convertToGeometryPoints( + const std::vector & polygon_points) +{ + std::vector points; + points.reserve(polygon_points.size()); + + for (const auto & polygon_point : polygon_points) { + points.push_back(polygon_point.point); + } + return points; +} + +std::vector concatenateTwoPolygons( + const std::vector & front_polygon, const std::vector & back_polygon) +{ + // At first, the front polygon is the outside polygon + bool is_front_polygon_outside = true; + size_t outside_idx = 0; + + const auto get_out_poly = [&]() { + return is_front_polygon_outside ? front_polygon : back_polygon; + }; + const auto get_in_poly = [&]() { + return is_front_polygon_outside ? back_polygon : front_polygon; + }; + + std::vector concatenated_polygon; + while (rclcpp::ok()) { + concatenated_polygon.push_back(get_out_poly().at(outside_idx)); + if (outside_idx == get_out_poly().size() - 1) { + break; + } + const size_t curr_idx = outside_idx; + const size_t next_idx = outside_idx + 1; + + for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { + const auto intersection = intersect( + get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, + get_in_poly().at(i).point, get_in_poly().at(i + 1).point); + if (intersection) { + const auto intersect_point = PolygonPoint{intersection.get(), 0, 0.0, 0.0}; + concatenated_polygon.push_back(intersect_point); + + is_front_polygon_outside = !is_front_polygon_outside; + outside_idx = i; + break; + } + } + outside_idx += 1; + } + + return concatenated_polygon; +} + +std::vector> concatenatePolygons( + const std::vector> & polygons) +{ + auto unique_polygons = polygons; + + while (rclcpp::ok()) { + bool is_updated = false; + + for (size_t i = 0; i < unique_polygons.size(); ++i) { + for (size_t j = 0; j < i; ++j) { + const auto & p1 = unique_polygons.at(i); + const auto & p2 = unique_polygons.at(j); + + // if p1 and p2 overlaps + if (p1.back().is_after(p2.front()) && p2.back().is_after(p1.front())) { + is_updated = true; + + const auto concatenated_polygon = [&]() { + if (p2.front().is_after(p1.front())) { + return concatenateTwoPolygons(p1, p2); + } + return concatenateTwoPolygons(p2, p1); + }(); + + // NOTE: remove i's element first since is larger than j. + unique_polygons.erase(unique_polygons.begin() + i); + unique_polygons.erase(unique_polygons.begin() + j); + + unique_polygons.push_back(concatenated_polygon); + break; + } + } + if (is_updated) { + break; + } + } + + if (!is_updated) { + break; + } + } + return unique_polygons; +} + +std::vector getPolygonPointsInsideBounds( + const std::vector & bound, const std::vector & edge_points, + const bool is_object_right) +{ + // NOTE: Polygon is defined at lest by three points. + if (edge_points.size() < 3) { + return std::vector(); + } + + // convert to vector of PolygonPoint + const auto inside_polygon = [&]() { + auto tmp_polygon = generatePolygonInsideBounds(bound, edge_points, is_object_right); + + // In order to make the order of points the same as the order of lon_dist_to_segment. + // The order of points is clockwise. + if (!is_object_right) { + std::reverse(tmp_polygon.begin(), tmp_polygon.end()); + } + return tmp_polygon; + }(); + if (inside_polygon.empty()) { + return std::vector(); + } + + // search start and end index by longitudinal distance + std::vector polygon_indices(inside_polygon.size()); + std::iota(polygon_indices.begin(), polygon_indices.end(), 0); + std::sort(polygon_indices.begin(), polygon_indices.end(), [&](int i1, int i2) { + return inside_polygon.at(i2).is_after(inside_polygon.at(i1)); + }); + const int start_idx = polygon_indices.front(); + const int end_idx = polygon_indices.back(); + + // calculate valid inside polygon + std::vector valid_inside_polygon; + for (int i = 0; i < (end_idx - start_idx + static_cast(polygon_indices.size())) % + static_cast(polygon_indices.size()) + + 1; + ++i) { + const int poly_idx = (start_idx + i) % static_cast(inside_polygon.size()); + valid_inside_polygon.push_back(inside_polygon.at(poly_idx)); + } + + // add start and end points projected to bound if necessary + if (inside_polygon.at(start_idx).lat_dist_to_bound != 0.0) { // not on bound + auto start_point = inside_polygon.at(start_idx); + const auto start_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( + bound, start_point.bound_seg_idx, start_point.lon_dist_to_segment); + if (start_point_on_bound) { + start_point.point = start_point_on_bound.get(); + valid_inside_polygon.insert(valid_inside_polygon.begin(), start_point); + } + } + if (inside_polygon.at(end_idx).lat_dist_to_bound != 0.0) { // not on bound + auto end_point = inside_polygon.at(end_idx); + const auto end_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( + bound, end_point.bound_seg_idx, end_point.lon_dist_to_segment); + if (end_point_on_bound) { + end_point.point = end_point_on_bound.get(); + valid_inside_polygon.insert(valid_inside_polygon.end(), end_point); + } + } + return valid_inside_polygon; +} + +std::vector updateBoundary( + const std::vector & original_bound, + const std::vector> & sorted_polygons) +{ + if (sorted_polygons.empty()) { + return original_bound; + } + + auto reversed_polygons = sorted_polygons; + std::reverse(reversed_polygons.begin(), reversed_polygons.end()); + + auto updated_bound = original_bound; + + // NOTE: Further obstacle is applied first since a part of the updated_bound is erased. + for (const auto & polygon : reversed_polygons) { + const auto & start_poly = polygon.front(); + const auto & end_poly = polygon.back(); + + const double front_offset = motion_utils::calcLongitudinalOffsetToSegment( + updated_bound, start_poly.bound_seg_idx, start_poly.point); + + const size_t removed_start_idx = + 0 < front_offset ? start_poly.bound_seg_idx + 1 : start_poly.bound_seg_idx; + const size_t removed_end_idx = end_poly.bound_seg_idx; + + updated_bound.erase( + updated_bound.begin() + removed_start_idx, updated_bound.begin() + removed_end_idx + 1); + + const auto obj_points = convertToGeometryPoints(polygon); + updated_bound.insert( + updated_bound.begin() + removed_start_idx, obj_points.begin(), obj_points.end()); + } + return updated_bound; +} + +[[maybe_unused]] geometry_msgs::msg::Point calcCenterOfGeometry(const Polygon2d & obj_poly) +{ + geometry_msgs::msg::Point center_pos; + for (const auto & point : obj_poly.outer()) { + center_pos.x += point.x(); + center_pos.y += point.y(); + } + + center_pos.x = center_pos.x / obj_poly.outer().size(); + center_pos.y = center_pos.y / obj_poly.outer().size(); + center_pos.z = center_pos.z / obj_poly.outer().size(); + + return center_pos; +} +} // namespace drivable_area_processing + +std::optional getPolygonByPoint( + const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, + const std::string & polygon_name) +{ + const auto polygons = route_handler->getLaneletMapPtr()->polygonLayer.findUsages(point); + for (const auto & polygon : polygons) { + const std::string type = polygon.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == polygon_name) { + // NOTE: If there are multiple polygons on a point, only the front one is used. + return polygon; + } + } + return std::nullopt; +} + double l2Norm(const Vector3 vector) { return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); @@ -601,6 +1000,142 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal return false; } +BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & planner_data) +{ + BehaviorModuleOutput output; + + const auto & p = planner_data->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & modified_goal = planner_data->prev_modified_goal; + + const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet goal_lane; + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (isInLanelets(goal_pose, shoulder_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + } + return !route_handler->getGoalLanelet(&goal_lane); + }); + if (is_failed_getting_lanelet) { + return output; + } + + constexpr double backward_length = 1.0; + const auto arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, goal_pose); + const double s_start = std::max(arc_coord.length - backward_length, 0.0); + const double s_end = arc_coord.length; + + auto reference_path = route_handler->getCenterLinePath({goal_lane}, s_start, s_end); + + const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); + const auto drivable_lanes = generateDrivableLanes(drivable_lanelets); + + const auto & dp = planner_data->drivable_area_expansion_parameters; + + const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + // for old architecture + generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data); + + output.path = std::make_shared(reference_path); + output.reference_path = std::make_shared(reference_path); + output.drivable_area_info.drivable_lanes = drivable_lanes; + + return output; +} + +bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + if (lanelet::utils::isInLanelet(pose, lane)) { + return true; + } + } + return false; +} + +bool isInLaneletWithYawThreshold( + const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, + const double radius) +{ + const double pose_yaw = tf2::getYaw(current_pose.orientation); + const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); + const double angle_diff = + std::abs(tier4_autoware_utils::normalizeRadian(lanelet_angle - pose_yaw)); + + return (angle_diff < std::abs(yaw_threshold)) && + lanelet::utils::isInLanelet(current_pose, lanelet, radius); +} + +bool isEgoOutOfRoute( + const Pose & self_pose, const std::optional & modified_goal, + const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid()) + ? modified_goal->pose + : route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet goal_lane; + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (utils::isInLanelets(goal_pose, shoulder_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + } + return !route_handler->getGoalLanelet(&goal_lane); + }); + if (is_failed_getting_lanelet) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet"); + return true; + } + + // If ego vehicle is over goal on goal lane, return true + const double yaw_threshold = tier4_autoware_utils::deg2rad(90); + if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { + constexpr double buffer = 1.0; + const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); + const auto goal_arc_coord = + lanelet::utils::getArcCoordinates({goal_lane}, route_handler->getGoalPose()); + if (ego_arc_coord.length > goal_arc_coord.length + buffer) { + return true; + } else { + return false; + } + } + + // If ego vehicle is out of the closest lanelet, return true + // Check if ego vehicle is in shoulder lane + const bool is_in_shoulder_lane = std::invoke([&]() { + lanelet::Lanelet closest_shoulder_lanelet; + if (!lanelet::utils::query::getClosestLanelet( + shoulder_lanes, self_pose, &closest_shoulder_lanelet)) { + return false; + } + return lanelet::utils::isInLanelet(self_pose, closest_shoulder_lanelet); + }); + // Check if ego vehicle is in road lane + const bool is_in_road_lane = std::invoke([&]() { + lanelet::ConstLanelet closest_road_lane; + if (!route_handler->getClosestLaneletWithinRoute(self_pose, &closest_road_lane)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("util"), + "cannot find closest road lanelet"); + return false; + } + return lanelet::utils::isInLanelet(self_pose, closest_road_lane); + }); + if (!is_in_shoulder_lane && !is_in_road_lane) { + return true; + } + + return false; +} + lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) { lanelet::ConstLanelets lanes; @@ -810,12 +1345,10 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( } void generateDrivableArea( - PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + PathWithLaneId & path, const std::vector & lanes, + const bool enable_expanding_polygon, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward) { - std::vector left_bound; - std::vector right_bound; - // extract data const auto transformed_lanes = utils::transformToLanelets(lanes); const auto current_pose = planner_data->self_odometry->pose.pose; @@ -849,9 +1382,15 @@ void generateDrivableArea( }; // Insert Position - for (const auto & lane : lanes) { - addPoints(lane.left_lane.leftBound3d(), left_bound); - addPoints(lane.right_lane.rightBound3d(), right_bound); + auto left_bound = calcBound(route_handler, lanes, enable_expanding_polygon, true); + auto right_bound = calcBound(route_handler, lanes, enable_expanding_polygon, false); + + if (left_bound.empty() || right_bound.empty()) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, + "The right or left bound of drivable area is empty"); + return; } // Insert points after goal @@ -894,21 +1433,38 @@ void generateDrivableArea( std::reverse(right_bound.begin(), right_bound.end()); } - // Get Closest segment for the start point - constexpr double front_length = 0.5; - const auto front_pose = path.points.empty() ? current_pose : path.points.front().point.pose; - const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); - const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); - 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); - const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); + path.left_bound.clear(); + path.right_bound.clear(); + + const auto [left_start_idx, right_start_idx] = [&]() { + const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); + const auto cropped_path_points = motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, + planner_data->parameters.forward_path_length, + planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); + + constexpr double front_length = 0.5; + 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); + const size_t front_right_start_idx = + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + 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); + const size_t right_start_idx = + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); + + // Insert a start point + path.left_bound.push_back(left_start_point); + path.right_bound.push_back(right_start_point); + + return std::make_pair(left_start_idx, right_start_idx); + }(); // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; @@ -926,14 +1482,6 @@ void generateDrivableArea( goal_right_start_idx, findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); - // Store Data - path.left_bound.clear(); - path.right_bound.clear(); - - // Insert a start point - path.left_bound.push_back(left_start_point); - path.right_bound.push_back(right_start_point); - // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { const auto & next_point = left_bound.at(i); @@ -967,12 +1515,200 @@ void generateDrivableArea( path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, transformed_lanes); } + + // make bound longitudinally monotonic + makeBoundLongitudinallyMonotonic(path, true); // for left bound + makeBoundLongitudinallyMonotonic(path, false); // for right bound +} + +// calculate bounds from drivable lanes and hatched road markings +std::vector calcBound( + const std::shared_ptr route_handler, + const std::vector & drivable_lanes, const bool enable_expanding_polygon, + const bool is_left) +{ + // a function to convert drivable lanes to points without duplicated points + const auto convert_to_points = [&](const std::vector & drivable_lanes) { + constexpr double overlap_threshold = 0.01; + + std::vector points; + for (const auto & drivable_lane : drivable_lanes) { + const auto bound = + is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); + for (const auto & point : bound) { + if ( + points.empty() || + overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { + points.push_back(point); + } + } + } + return points; + }; + // a function to get polygon with a designated id + const auto get_corresponding_polygon_index = [&](const auto & polygon, const auto & target_id) { + for (size_t poly_idx = 0; poly_idx < polygon.size(); ++poly_idx) { + if (polygon[poly_idx].id() == target_id) { + // NOTE: If there are duplicated points in polygon, the early one will be returned. + return poly_idx; + } + } + // This means calculation has some errors. + return polygon.size() - 1; + }; + const auto mod = [&](const int a, const int b) { + return (a + b) % b; // NOTE: consider negative value + }; + + // If no need to expand with polygons, return here. + std::vector output_points; + const auto bound_points = convert_to_points(drivable_lanes); + if (!enable_expanding_polygon) { + for (const auto & point : bound_points) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); + } + return motion_utils::removeOverlapPoints(output_points); + } + + std::optional current_polygon{std::nullopt}; + std::vector current_polygon_border_indices; + for (size_t point_idx = 0; point_idx < bound_points.size(); ++point_idx) { + const auto & point = bound_points.at(point_idx); + const auto polygon = getPolygonByPoint(route_handler, point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, point.id())); + } + } else { + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, point.id())); + } + } + + if (point_idx == bound_points.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); + } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); + } + } + + return motion_utils::removeOverlapPoints(output_points); +} + +void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left) +{ + if (path.points.empty()) { + return; + } + + // define a function to remove non monotonic point on bound + const auto remove_non_monotonic_point = + [&](std::vector original_bound, const bool is_reversed) { + if (is_reversed) { + std::reverse(original_bound.begin(), original_bound.end()); + } + + const bool is_points_left = is_reversed ? !is_bound_left : is_bound_left; + + std::vector monotonic_bound; + size_t b_idx = 0; + while (true) { + if (original_bound.size() <= b_idx) { + break; + } + monotonic_bound.push_back(original_bound.at(b_idx)); + + // calculate bound pose and its laterally offset pose. + const auto bound_pose = [&]() { + geometry_msgs::msg::Pose pose; + pose.position = original_bound.at(b_idx); + const size_t nearest_idx = + motion_utils::findNearestIndex(path.points, original_bound.at(b_idx)); + pose.orientation = path.points.at(nearest_idx).point.pose.orientation; + return pose; + }(); + // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is + // opposite. + const double lat_offset = is_bound_left ? 20.0 : -20.0; + const auto bound_pose_with_lat_offset = + tier4_autoware_utils::calcOffsetPose(bound_pose, 0.0, lat_offset, 0.0); + + // skip non monotonic points + for (size_t candidate_idx = b_idx + 1; candidate_idx < original_bound.size() - 1; + ++candidate_idx) { + const auto intersect_point = drivable_area_processing::intersect( + bound_pose.position, bound_pose_with_lat_offset.position, + original_bound.at(candidate_idx), original_bound.at(candidate_idx + 1)); + + if (intersect_point) { + const double theta = tier4_autoware_utils::normalizeRadian( + tier4_autoware_utils::calcAzimuthAngle( + bound_pose.position, original_bound.at(candidate_idx)) - + tier4_autoware_utils::calcAzimuthAngle( + bound_pose.position, bound_pose_with_lat_offset.position)); + if ((is_points_left && 0 < theta) || (!is_points_left && theta < 0)) { + monotonic_bound.push_back(*intersect_point); + b_idx = candidate_idx; + break; + } + } + } + + ++b_idx; + } + + if (is_reversed) { + std::reverse(monotonic_bound.begin(), monotonic_bound.end()); + } + return monotonic_bound; + }; + + auto & bound = is_bound_left ? path.left_bound : path.right_bound; + const auto original_bound = bound; + const auto half_monotonic_bound = + remove_non_monotonic_point(original_bound, true); // for reverse + const auto full_monotonic_bound = + remove_non_monotonic_point(half_monotonic_bound, false); // for not reverse + + bound = full_monotonic_bound; } // generate drivable area by expanding path for freespace void generateDrivableArea( - PathWithLaneId & path, const double vehicle_length, const double vehicle_width, - const double margin, const bool is_driving_forward) + PathWithLaneId & path, const double vehicle_length, const double offset, + const bool is_driving_forward) { using tier4_autoware_utils::calcOffsetPose; @@ -1007,8 +1743,8 @@ void generateDrivableArea( for (const auto & point : resampled_path.points) { const auto & pose = point.point.pose; - const auto left_point = calcOffsetPose(pose, 0, vehicle_width / 2.0 + margin, 0); - const auto right_point = calcOffsetPose(pose, 0, -vehicle_width / 2.0 - margin, 0); + const auto left_point = calcOffsetPose(pose, 0, offset, 0); + const auto right_point = calcOffsetPose(pose, 0, -offset, 0); left_bound.push_back(left_point.position); right_bound.push_back(right_point.position); @@ -1018,32 +1754,32 @@ void generateDrivableArea( // add backward offset point to bound const Pose first_point = calcOffsetPose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0); - const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, offset, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -offset, 0); left_bound.insert(left_bound.begin(), left_first_point.position); right_bound.insert(right_bound.begin(), right_first_point.position); // add forward offset point to bound const Pose last_point = calcOffsetPose(resampled_path.points.back().point.pose, vehicle_length, 0, 0); - const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, offset, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -offset, 0); left_bound.push_back(left_last_point.position); right_bound.push_back(right_last_point.position); } else { // add forward offset point to bound const Pose first_point = calcOffsetPose(resampled_path.points.front().point.pose, vehicle_length, 0, 0); - const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, offset, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -offset, 0); left_bound.insert(left_bound.begin(), left_first_point.position); right_bound.insert(right_bound.begin(), right_first_point.position); // add backward offset point to bound const Pose last_point = calcOffsetPose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0); - const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, offset, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -offset, 0); left_bound.push_back(left_last_point.position); right_bound.push_back(right_last_point.position); } @@ -1116,26 +1852,57 @@ double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLan double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { - const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); - lanelet::ConstLanelet current_lanelet; if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { - return std::numeric_limits::max(); + return std::numeric_limits::infinity(); } - double distance = 0; - bool is_after_current_lanelet = false; + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); + const auto to_object = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(current_lanelet.centerline()), + lanelet::utils::to2D(lanelet_point).basicPoint()); + + for (const auto & element : current_lanelet.regulatoryElementsAs()) { + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + + const auto to_stop_line = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(current_lanelet.centerline()), + lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); + + const auto distance_object_to_stop_line = to_stop_line.length - to_object.length; + + if (distance_object_to_stop_line > 0.0) { + return distance_object_to_stop_line; + } + } + + double distance = lanelet::utils::getLaneletLength3d(current_lanelet); + + bool found_current_lane = false; for (const auto & llt : lanelets) { - if (llt == current_lanelet) { - is_after_current_lanelet = true; + if (llt.id() == current_lanelet.id()) { + found_current_lane = true; + continue; } - if (is_after_current_lanelet && !llt.regulatoryElementsAs().empty()) { - return distance - arc_coordinates.length; + + if (!found_current_lane) { + continue; } + + for (const auto & element : llt.regulatoryElementsAs()) { + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + + const auto to_stop_line = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(llt.centerline()), + lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); + + return distance + to_stop_line.length - to_object.length; + } + distance += lanelet::utils::getLaneletLength3d(llt); } - return std::numeric_limits::max(); + return std::numeric_limits::infinity(); } double getDistanceToNextIntersection( @@ -1188,7 +1955,7 @@ double getDistanceToCrosswalk( lanelet::ConstLanelet current_lanelet; if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { - return std::numeric_limits::max(); + return std::numeric_limits::infinity(); } double distance = 0; @@ -1197,24 +1964,8 @@ double getDistanceToCrosswalk( if (llt == current_lanelet) { is_after_current_lanelet = true; } - // check lane change tag - bool is_lane_change_yes = false; - const auto right_line = llt.rightBound(); - if (right_line.hasAttribute(lanelet::AttributeNamesString::LaneChange)) { - const auto attr = right_line.attribute(lanelet::AttributeNamesString::LaneChange); - if (attr.value() == std::string("yes")) { - is_lane_change_yes = true; - } - } - const auto left_line = llt.leftBound(); - if (left_line.hasAttribute(lanelet::AttributeNamesString::LaneChange)) { - const auto attr = left_line.attribute(lanelet::AttributeNamesString::LaneChange); - if (attr.value() == std::string("yes")) { - is_lane_change_yes = true; - } - } - if (is_after_current_lanelet && !is_lane_change_yes) { + if (is_after_current_lanelet) { const auto conflicting_crosswalks = overall_graphs.conflictingInGraph(llt, 1); if (!(conflicting_crosswalks.empty())) { // create centerline @@ -1226,7 +1977,7 @@ double getDistanceToCrosswalk( } // create crosswalk polygon and calculate distance - double min_distance_to_crosswalk = std::numeric_limits::max(); + double min_distance_to_crosswalk = std::numeric_limits::infinity(); for (const auto & crosswalk : conflicting_crosswalks) { lanelet::CompoundPolygon2d lanelet_crosswalk_polygon = crosswalk.polygon2d(); Polygon2d polygon; @@ -1261,7 +2012,7 @@ double getDistanceToCrosswalk( distance += lanelet::utils::getLaneletLength3d(llt); } - return std::numeric_limits::max(); + return std::numeric_limits::infinity(); } double getSignedDistance( @@ -1323,18 +2074,18 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) return path.points.at(*insert_idx); } -double getSignedDistanceFromShoulderLeftBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose) +double getSignedDistanceFromBoundary( + const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) { - lanelet::ConstLanelet closest_shoulder_lanelet; + lanelet::ConstLanelet closest_lanelet; lanelet::ArcCoordinates arc_coordinates; - if (lanelet::utils::query::getClosestLanelet( - shoulder_lanelets, pose, &closest_shoulder_lanelet)) { + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & left_line_2d = lanelet::utils::to2D(closest_shoulder_lanelet.leftBound3d()); + const auto & boundary_line_2d = left_side + ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) + : lanelet::utils::to2D(closest_lanelet.rightBound3d()); arc_coordinates = lanelet::geometry::toArcCoordinates( - left_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); } else { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("utils"), @@ -1344,29 +2095,30 @@ double getSignedDistanceFromShoulderLeftBoundary( return arc_coordinates.distance; } -std::optional getSignedDistanceFromShoulderLeftBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose) +std::optional getSignedDistanceFromBoundary( + const lanelet::ConstLanelets & lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose, const bool left_side) { double min_distance = std::numeric_limits::max(); + const auto transformed_footprint = transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); - bool found_neighbor_shoulder_bound = false; + bool found_neighbor_bound = false; for (const auto & vehicle_corner_point : transformed_footprint) { // convert point of footprint to pose Pose vehicle_corner_pose{}; vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); vehicle_corner_pose.orientation = vehicle_pose.orientation; - // calculate distance to the shoulder bound directly next to footprint points - lanelet::ConstLanelet closest_shoulder_lanelet{}; - if (lanelet::utils::query::getClosestLanelet( - shoulder_lanelets, vehicle_corner_pose, &closest_shoulder_lanelet)) { - const auto & left_line_2d = lanelet::utils::to2D(closest_shoulder_lanelet.leftBound3d()); + // calculate distance to the bound directly next to footprint points + lanelet::ConstLanelet closest_lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanelets, vehicle_corner_pose, &closest_lanelet)) { + const auto & bound_line_2d = left_side ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) + : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - for (size_t i = 1; i < left_line_2d.size(); ++i) { - const Point p_front = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i - 1]); - const Point p_back = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i]); + for (size_t i = 1; i < bound_line_2d.size(); ++i) { + const Point p_front = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i - 1]); + const Point p_back = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); const Point inverse_p_front = tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); @@ -1381,43 +2133,29 @@ std::optional getSignedDistanceFromShoulderLeftBoundary( if (dx_front < 0 && dx_back > 0) { const double lateral_distance_from_pose_to_segment = (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); - min_distance = std::min(lateral_distance_from_pose_to_segment, min_distance); - found_neighbor_shoulder_bound = true; + + if (std::abs(lateral_distance_from_pose_to_segment) < std::abs(min_distance)) { + min_distance = lateral_distance_from_pose_to_segment; + } + + found_neighbor_bound = true; break; } } } } - if (!found_neighbor_shoulder_bound) { + if (!found_neighbor_bound) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("utils"), "neighbor shoulder bound to footprint is not found."); return {}; } + // min_distance is the distance from corner_pose to bound, so reverse this value return -min_distance; } -double getSignedDistanceFromRightBoundary( - const lanelet::ConstLanelets & lanelets, const Pose & pose) -{ - lanelet::ConstLanelet closest_lanelet; - lanelet::ArcCoordinates arc_coordinates; - if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & right_line_2d = lanelet::utils::to2D(closest_lanelet.rightBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - right_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "closest shoulder lanelet not found."); - } - - return arc_coordinates.distance; -} - double getArcLengthToTargetLanelet( const lanelet::ConstLanelets & lanelet_sequence, const lanelet::ConstLanelet & target_lane, const Pose & pose) @@ -1531,7 +2269,8 @@ std::shared_ptr generateCenterLinePath( centerline_path->header = route_handler->getRouteHeader(); - utils::generateDrivableArea(*centerline_path, drivable_lanes, p.vehicle_length, planner_data); + utils::generateDrivableArea( + *centerline_path, drivable_lanes, false, p.vehicle_length, planner_data); return centerline_path; } @@ -1653,20 +2392,22 @@ PathWithLaneId getCenterLinePath( const double s_backward = std::max(0., s - backward_path_length); double s_forward = s + forward_path_length; - const int num_lane_change = - std::abs(route_handler.getNumLaneToPreferredLane(lanelet_sequence.back())); const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(lanelet_sequence.back()); const double lane_change_buffer = - calcLaneChangeBuffer(parameter, std::abs(num_lane_change), optional_length); + utils::calcMinimumLaneChangeLength(parameter, shift_intervals, optional_length); if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) { - s_forward = std::min(s_forward, lane_length - lane_change_buffer); + const double forward_length = std::max(lane_length - lane_change_buffer, 0.0); + s_forward = std::min(s_forward, forward_length); } if (route_handler.isInGoalRouteSection(lanelet_sequence.back())) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, route_handler.getGoalPose()); - s_forward = std::min(s_forward, goal_arc_coordinates.length - lane_change_buffer); + const double forward_length = std::max(goal_arc_coordinates.length - lane_change_buffer, 0.0); + s_forward = std::min(s_forward, forward_length); } const auto raw_path_with_lane_id = @@ -1740,7 +2481,6 @@ PathWithLaneId setDecelerationVelocity( BehaviorModuleOutput getReferencePath( const lanelet::ConstLanelet & current_lane, - const std::shared_ptr & parameters, const std::shared_ptr & planner_data) { PathWithLaneId reference_path{}; @@ -1752,14 +2492,6 @@ BehaviorModuleOutput getReferencePath( // Set header reference_path.header = route_handler->getRouteHeader(); - // For current_lanes with desired length - const auto current_lanes = route_handler->getLaneletSequence( - current_lane, current_pose, p.backward_path_length, p.forward_path_length); - - if (current_lanes.empty()) { - return {}; - } - // calculate path with backward margin to avoid end points' instability by spline interpolation constexpr double extra_margin = 10.0; const double backward_length = p.backward_path_length + extra_margin; @@ -1780,29 +2512,20 @@ BehaviorModuleOutput getReferencePath( const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); const auto drivable_lanes = generateDrivableLanes(drivable_lanelets); - { - const int num_lane_change = - std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - - const double lane_change_buffer = calcLaneChangeBuffer(p, num_lane_change); - - reference_path = setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters->lane_change_prepare_duration, - lane_change_buffer); - } + const auto & dp = planner_data->drivable_area_expansion_parameters; const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = expandLanelets( - shorten_lanes, parameters->drivable_area_left_bound_offset, - parameters->drivable_area_right_bound_offset, parameters->drivable_area_types_to_skip); + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); - generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data); + // for old architecture + generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data); BehaviorModuleOutput output; output.path = std::make_shared(reference_path); output.reference_path = std::make_shared(reference_path); - output.drivable_lanes = drivable_lanes; + output.drivable_area_info.drivable_lanes = drivable_lanes; return output; } @@ -1996,20 +2719,28 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } -double calcTotalLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const bool include_buffer) -{ - const double minimum_lane_change_distance = - common_param.minimum_prepare_length + common_param.minimum_lane_changing_length; - const double end_of_lane_buffer = common_param.backward_length_buffer_for_end_of_lane; - return minimum_lane_change_distance + end_of_lane_buffer * static_cast(include_buffer); -} - -double calcLaneChangeBuffer( - const BehaviorPathPlannerParameters & common_param, const int num_lane_change, +double calcMinimumLaneChangeLength( + const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, const double length_to_intersection) { - return num_lane_change * calcTotalLaneChangeLength(common_param) + length_to_intersection; + if (shift_intervals.empty()) { + return 0.0; + } + + const double & vel = common_param.minimum_lane_changing_velocity; + const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); + const double & max_lateral_acc = lat_acc.second; + const double & lateral_jerk = common_param.lane_changing_lateral_jerk; + + double accumulated_length = + length_to_intersection + common_param.backward_length_buffer_for_end_of_lane; + for (const auto & shift_interval : shift_intervals) { + const double t = + PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); + accumulated_length += vel * t + common_param.minimum_prepare_length; + } + + return accumulated_length; } lanelet::ConstLanelets getLaneletsFromPath( @@ -2048,4 +2779,171 @@ std::string convertToSnakeCase(const std::string & input_str) } return output_str; } + +std::vector combineDrivableLanes( + const std::vector & original_drivable_lanes_vec, + const std::vector & new_drivable_lanes_vec) +{ + const auto has_same_lane = + [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { + return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { + return ll.id() == target_lane.id(); + }) != lanes.end(); + }; + + const auto convert_to_lanes = [](const DrivableLanes & drivable_lanes) { + auto lanes = drivable_lanes.middle_lanes; + lanes.push_back(drivable_lanes.right_lane); + lanes.push_back(drivable_lanes.left_lane); + return lanes; + }; + + auto updated_drivable_lanes_vec = original_drivable_lanes_vec; + size_t new_drivable_lanes_idx = 0; + for (auto & updated_drivable_lanes : updated_drivable_lanes_vec) { + // calculated corresponding index of new_drivable_lanes + const auto opt_new_drivable_lanes_idx = [&]() -> std::optional { + for (size_t n_idx = 0; n_idx < new_drivable_lanes_vec.size(); ++n_idx) { + for (const auto & ll : convert_to_lanes(updated_drivable_lanes)) { + if (has_same_lane(ll, convert_to_lanes(new_drivable_lanes_vec.at(n_idx)))) { + return n_idx; + } + } + } + return std::nullopt; + }(); + if (!opt_new_drivable_lanes_idx) { + continue; + } + new_drivable_lanes_idx = *opt_new_drivable_lanes_idx; + const auto & new_drivable_lanes = new_drivable_lanes_vec.at(new_drivable_lanes_idx); + + // update left lane + if (has_same_lane(updated_drivable_lanes.left_lane, convert_to_lanes(new_drivable_lanes))) { + updated_drivable_lanes.left_lane = new_drivable_lanes.left_lane; + } + // update right lane + if (has_same_lane(updated_drivable_lanes.right_lane, convert_to_lanes(new_drivable_lanes))) { + updated_drivable_lanes.right_lane = new_drivable_lanes.right_lane; + } + // update middle lanes + for (const auto & middle_lane : convert_to_lanes(new_drivable_lanes)) { + if (!has_same_lane(middle_lane, convert_to_lanes(updated_drivable_lanes))) { + updated_drivable_lanes.middle_lanes.push_back(middle_lane); + } + } + + // validate middle lanes + auto & middle_lanes = updated_drivable_lanes.middle_lanes; + if (has_same_lane(updated_drivable_lanes.right_lane, middle_lanes)) { + middle_lanes.erase( + std::remove( + std::begin(middle_lanes), std::end(middle_lanes), updated_drivable_lanes.right_lane), + std::cend(middle_lanes)); + } + if (has_same_lane(updated_drivable_lanes.left_lane, middle_lanes)) { + middle_lanes.erase( + std::remove( + std::begin(middle_lanes), std::end(middle_lanes), updated_drivable_lanes.left_lane), + std::cend(middle_lanes)); + } + } + // NOTE: If original_drivable_lanes_vec is shorter than new_drivable_lanes_vec, push back remained + // new_drivable_lanes_vec. + updated_drivable_lanes_vec.insert( + updated_drivable_lanes_vec.end(), new_drivable_lanes_vec.begin() + new_drivable_lanes_idx + 1, + new_drivable_lanes_vec.end()); + + return updated_drivable_lanes_vec; +} + +DrivableAreaInfo combineDrivableAreaInfo( + const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2) +{ + DrivableAreaInfo combined_drivable_area_info; + + // drivable lanes +#ifndef USE_OLD_ARCHITECTURE + combined_drivable_area_info.drivable_lanes = + combineDrivableLanes(drivable_area_info1.drivable_lanes, drivable_area_info2.drivable_lanes); +#endif + + // obstacles + for (const auto & obstacle : drivable_area_info1.obstacles) { + combined_drivable_area_info.obstacles.push_back(obstacle); + } + for (const auto & obstacle : drivable_area_info2.obstacles) { + combined_drivable_area_info.obstacles.push_back(obstacle); + } + + // enable expanding hatched road markings + combined_drivable_area_info.enable_expanding_hatched_road_markings = + drivable_area_info1.enable_expanding_hatched_road_markings || + drivable_area_info2.enable_expanding_hatched_road_markings; + + return combined_drivable_area_info; +} + +// NOTE: Assuming that path.right/left_bound is already created. +void extractObstaclesFromDrivableArea( + PathWithLaneId & path, const std::vector & obstacles) +{ + if (obstacles.empty()) { + return; + } + + std::vector> right_polygons; + std::vector> left_polygons; + for (const auto & obstacle : obstacles) { + 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; + ++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(), + path.points.at(nearest_path_idx).point.pose.position.z)); + } + + // get a boundary that we have to change + const bool is_object_right = !obstacle.is_left; + const auto & bound = is_object_right ? path.right_bound : path.left_bound; + + // get polygon points inside the bounds + const auto inside_polygon = + drivable_area_processing::getPolygonPointsInsideBounds(bound, edge_points, is_object_right); + if (!inside_polygon.empty()) { + if (is_object_right) { + right_polygons.push_back(inside_polygon); + } else { + left_polygons.push_back(inside_polygon); + } + } + } + + for (const bool is_object_right : {true, false}) { + const auto & polygons = is_object_right ? right_polygons : left_polygons; + if (polygons.empty()) { + continue; + } + + // concatenate polygons if they are longitudinal overlapped. + auto unique_polygons = drivable_area_processing::concatenatePolygons(polygons); + + // sort bounds longitudinally + std::sort( + unique_polygons.begin(), unique_polygons.end(), + [](const std::vector & p1, const std::vector & p2) { + return p2.front().is_after(p1.front()); + }); + + // update boundary + auto & bound = is_object_right ? path.right_bound : path.left_bound; + bound = drivable_area_processing::updateBoundary(bound, unique_polygons); + } +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/test/input.cpp b/planning/behavior_path_planner/test/input.cpp index 83bfda132aed8..c2735abd3e932 100644 --- a/planning/behavior_path_planner/test/input.cpp +++ b/planning/behavior_path_planner/test/input.cpp @@ -32,7 +32,7 @@ PathWithLaneId generateStraightSamplePathWithLaneId( PathPointWithLaneId path_point_with_lane_id; path_point_with_lane_id.point = point; - path_point_with_lane_id.lane_ids = std::vector(); + path_point_with_lane_id.lane_ids = std::vector(); path.header.frame_id = "map"; path.points.push_back(path_point_with_lane_id); @@ -56,7 +56,7 @@ PathWithLaneId generateDiagonalSamplePathWithLaneId( PathPointWithLaneId path_point_with_lane_id; path_point_with_lane_id.point = point; - path_point_with_lane_id.lane_ids = std::vector(); + path_point_with_lane_id.lane_ids = std::vector(); path.header.frame_id = "map"; path.points.push_back(path_point_with_lane_id); diff --git a/planning/behavior_path_planner/test/test_avoidance_utils.cpp b/planning/behavior_path_planner/test/test_avoidance_utils.cpp index 1d8bb5ae72bdb..fc2bea1a5b288 100644 --- a/planning/behavior_path_planner/test/test_avoidance_utils.cpp +++ b/planning/behavior_path_planner/test/test_avoidance_utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/util.hpp" +#include "behavior_path_planner/utils/avoidance/utils.hpp" #include #include -using behavior_path_planner::isOnRight; -using behavior_path_planner::isSameDirectionShift; using behavior_path_planner::ObjectData; +using behavior_path_planner::utils::avoidance::isOnRight; +using behavior_path_planner::utils::avoidance::isSameDirectionShift; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { 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 5b9369c0478cb..a24086a3df8d8 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,14 +22,27 @@ #include #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() { - rclcpp::init(0, nullptr); + auto test_manager = std::make_shared(); - auto test_manager = std::make_shared(); + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - auto node_options = rclcpp::NodeOptions{}; + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_path_planner_dir = @@ -39,23 +52,28 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) "bt_tree_config_path", behavior_path_planner_dir + "/config/behavior_path_planner_tree.xml"); test_utils::updateNodeOptions( - node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", - behavior_path_planner_dir + "/config/lane_following/lane_following.param.yaml", - behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", - behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml", - behavior_path_planner_dir + "/config/pull_over/pull_over.param.yaml", - behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", - behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); - - auto test_target_node = - std::make_shared(node_options); + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", + behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", + behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", + behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml", + behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", + behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", + behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); @@ -69,18 +87,40 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); test_manager->publishLateralOffset( test_target_node, "behavior_path_planner/input/lateral_offset"); +} - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - test_manager->testWithAbnormalRoute(test_target_node); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index d126fc0607a71..21538de073d35 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -11,27 +11,98 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/utils.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include -TEST(BehaviorPathPlanningLaneChangeUtilsTest, testStoppingDistance) +constexpr double epsilon = 1e-6; + +TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) { - const auto vehicle_velocity = 8.333; - - const auto negative_accel = -1.5; - const auto distance_when_negative = - behavior_path_planner::utils::lane_change::stoppingDistance(vehicle_velocity, negative_accel); - ASSERT_NEAR(distance_when_negative, 23.1463, 1e-3); - - const auto positive_accel = 1.5; - const auto distance_when_positive = - behavior_path_planner::utils::lane_change::stoppingDistance(vehicle_velocity, positive_accel); - ASSERT_NEAR(distance_when_positive, 34.7194, 1e-3); - - const auto zero_accel = 0.0; - const auto distance_when_zero = - behavior_path_planner::utils::lane_change::stoppingDistance(vehicle_velocity, zero_accel); - ASSERT_NEAR(distance_when_zero, 34.7194, 1e-3); + geometry_msgs::msg::Pose ego_pose; + const auto ego_yaw = tier4_autoware_utils::deg2rad(0.0); + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.position = tier4_autoware_utils::createPoint(0, 0, 0); + + geometry_msgs::msg::Pose obj_pose; + const auto obj_yaw = tier4_autoware_utils::deg2rad(0.0); + obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(obj_yaw); + obj_pose.position = tier4_autoware_utils::createPoint(-4, 3, 0); + + const auto result = tier4_autoware_utils::inverseTransformPose(obj_pose, ego_pose); + + EXPECT_NEAR(result.position.x, -4, epsilon); + EXPECT_NEAR(result.position.y, 3, epsilon); +} + +TEST(BehaviorPathPlanningLaneChangeUtilsTest, TESTLateralAccelerationMap) +{ + LateralAccelerationMap lat_acc_map; + lat_acc_map.add(0.0, 0.2, 0.315); + lat_acc_map.add(3.0, 0.2, 0.315); + lat_acc_map.add(5.0, 0.2, 0.315); + lat_acc_map.add(6.0, 0.315, 0.40); + lat_acc_map.add(10.0, 0.315, 0.50); + + { + const auto [min_acc, max_acc] = lat_acc_map.find(-1.0); + EXPECT_NEAR(min_acc, 0.2, epsilon); + EXPECT_NEAR(max_acc, 0.315, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(0.0); + EXPECT_NEAR(min_acc, 0.2, epsilon); + EXPECT_NEAR(max_acc, 0.315, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(1.0); + EXPECT_NEAR(min_acc, 0.2, epsilon); + EXPECT_NEAR(max_acc, 0.315, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(3.0); + EXPECT_NEAR(min_acc, 0.2, epsilon); + EXPECT_NEAR(max_acc, 0.315, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(5.0); + EXPECT_NEAR(min_acc, 0.2, epsilon); + EXPECT_NEAR(max_acc, 0.315, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(5.5); + EXPECT_NEAR(min_acc, 0.2575, epsilon); + EXPECT_NEAR(max_acc, 0.3575, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(6.0); + EXPECT_NEAR(min_acc, 0.315, epsilon); + EXPECT_NEAR(max_acc, 0.4, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(8.0); + EXPECT_NEAR(min_acc, 0.315, epsilon); + EXPECT_NEAR(max_acc, 0.45, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(10.0); + EXPECT_NEAR(min_acc, 0.315, epsilon); + EXPECT_NEAR(max_acc, 0.50, epsilon); + } + + { + const auto [min_acc, max_acc] = lat_acc_map.find(11.0); + EXPECT_NEAR(min_acc, 0.315, epsilon); + EXPECT_NEAR(max_acc, 0.50, epsilon); + } } diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp new file mode 100644 index 0000000000000..07762ff96eb67 --- /dev/null +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -0,0 +1,188 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/safety_check.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +using autoware_auto_perception_msgs::msg::Shape; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using marker_utils::CollisionCheckDebug; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) +{ + using behavior_path_planner::utils::safety_check::createExtendedPolygon; + + vehicle_info_util::VehicleInfo vehicle_info; + vehicle_info.max_longitudinal_offset_m = 4.0; + vehicle_info.vehicle_width_m = 2.0; + vehicle_info.rear_overhang_m = 1.0; + + { + Pose ego_pose; + ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + + const double lon_length = 10.0; + const double lat_margin = 2.0; + + const auto polygon = createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin); + + EXPECT_EQ(polygon.outer().size(), static_cast(5)); + + const auto & p1 = polygon.outer().at(0); + const auto & p2 = polygon.outer().at(1); + const auto & p3 = polygon.outer().at(2); + const auto & p4 = polygon.outer().at(3); + EXPECT_NEAR(p1.x(), 14.0, epsilon); + EXPECT_NEAR(p1.y(), 3.0, epsilon); + EXPECT_NEAR(p2.x(), 14.0, epsilon); + EXPECT_NEAR(p2.y(), -3.0, epsilon); + EXPECT_NEAR(p3.x(), -1.0, epsilon); + EXPECT_NEAR(p3.y(), -3.0, epsilon); + EXPECT_NEAR(p4.x(), -1.0, epsilon); + EXPECT_NEAR(p4.y(), 3.0, epsilon); + } + + { + Pose ego_pose; + ego_pose.position = tier4_autoware_utils::createPoint(3.0, 4.0, 0.0); + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + + const double lon_length = 10.0; + const double lat_margin = 2.0; + + const auto polygon = createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin); + + EXPECT_EQ(polygon.outer().size(), static_cast(5)); + + const auto & p1 = polygon.outer().at(0); + const auto & p2 = polygon.outer().at(1); + const auto & p3 = polygon.outer().at(2); + const auto & p4 = polygon.outer().at(3); + EXPECT_NEAR(p1.x(), 17.0, epsilon); + EXPECT_NEAR(p1.y(), 7.0, epsilon); + EXPECT_NEAR(p2.x(), 17.0, epsilon); + EXPECT_NEAR(p2.y(), 1.0, epsilon); + EXPECT_NEAR(p3.x(), 2.0, epsilon); + EXPECT_NEAR(p3.y(), 1.0, epsilon); + EXPECT_NEAR(p4.x(), 2.0, epsilon); + EXPECT_NEAR(p4.y(), 7.0, epsilon); + } + + { + Pose ego_pose; + ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::deg2rad(60)); + + const double lon_length = 10.0; + const double lat_margin = 2.0; + + const auto polygon = createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin); + + EXPECT_EQ(polygon.outer().size(), static_cast(5)); + + const auto & p1 = polygon.outer().at(0); + const auto & p2 = polygon.outer().at(1); + const auto & p3 = polygon.outer().at(2); + const auto & p4 = polygon.outer().at(3); + EXPECT_NEAR(p1.x(), 7.0 - 1.5 * std::sqrt(3), epsilon); + EXPECT_NEAR(p1.y(), 7.0 * std::sqrt(3) + 1.5, epsilon); + EXPECT_NEAR(p2.x(), 7.0 + 1.5 * std::sqrt(3), epsilon); + EXPECT_NEAR(p2.y(), 7.0 * std::sqrt(3) - 1.5, epsilon); + EXPECT_NEAR(p3.x(), 1.5 * std::sqrt(3) - 0.5, epsilon); + EXPECT_NEAR(p3.y(), -1.5 - std::sqrt(3) / 2.0, epsilon); + EXPECT_NEAR(p4.x(), -1.5 * std::sqrt(3) - 0.5, epsilon); + EXPECT_NEAR(p4.y(), 1.5 - std::sqrt(3) / 2.0, epsilon); + } +} + +TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) +{ + using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::createQuaternionFromYaw; + + { + Pose obj_pose; + obj_pose.position = createPoint(0.0, 0.0, 0.0); + obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + + Shape shape; + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + shape.footprint.points.resize(5); + shape.footprint.points.at(0).x = 3.0; + shape.footprint.points.at(0).y = 0.0; + shape.footprint.points.at(1).x = 0.0; + shape.footprint.points.at(1).y = -2.0; + shape.footprint.points.at(2).x = -2.0; + shape.footprint.points.at(2).y = 0.0; + shape.footprint.points.at(3).x = -1.0; + shape.footprint.points.at(3).y = 0.5; + shape.footprint.points.at(4).x = 2.0; + shape.footprint.points.at(4).y = 1.0; + + const double lon_length = 10.0; + const double lat_margin = 2.0; + + const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin); + + EXPECT_EQ(polygon.outer().size(), static_cast(5)); + + const auto & p1 = polygon.outer().at(0); + const auto & p2 = polygon.outer().at(1); + const auto & p3 = polygon.outer().at(2); + const auto & p4 = polygon.outer().at(3); + EXPECT_NEAR(p1.x(), 13.0, epsilon); + EXPECT_NEAR(p1.y(), 3.0, epsilon); + EXPECT_NEAR(p2.x(), 13.0, epsilon); + EXPECT_NEAR(p2.y(), -4.0, epsilon); + EXPECT_NEAR(p3.x(), -2.0, epsilon); + EXPECT_NEAR(p3.y(), -4.0, epsilon); + EXPECT_NEAR(p4.x(), -2.0, epsilon); + EXPECT_NEAR(p4.y(), 3.0, epsilon); + } +} + +TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) +{ + using behavior_path_planner::utils::safety_check::calcRssDistance; + + { + const double front_vel = 5.0; + const double front_decel = -2.0; + const double rear_vel = 10.0; + const double rear_decel = -1.0; + BehaviorPathPlannerParameters params; + params.rear_vehicle_reaction_time = 1.0; + params.rear_vehicle_safety_time_margin = 1.0; + params.longitudinal_distance_min_threshold = 3.0; + + EXPECT_NEAR( + calcRssDistance(front_vel, rear_vel, front_decel, rear_decel, params), 63.75, epsilon); + } +} diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt new file mode 100644 index 0000000000000..da4a9de4a2a8b --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_blind_spot_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml new file mode 100644 index 0000000000000..9485f165849f2 --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -0,0 +1,40 @@ + + + + behavior_velocity_blind_spot_module + 0.1.0 + The behavior_velocity_blind_spot_module package + + Mamoru Sobue + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml new file mode 100644 index 0000000000000..7dda59ea2fdbe --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp rename to planning/behavior_velocity_blind_spot_module/src/debug.cpp index ddef8d7fff3e7..32c91d2ac47ed 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include #include -#include -#include -#include #include @@ -115,7 +116,7 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr if (!isActivated() && !is_over_pass_judge_line_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.virtual_wall_pose}, "blind_spot", now, module_id_), + {debug_data_.virtual_wall_pose}, "blind_spot", now), &wall_marker, now); } return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp similarity index 89% rename from planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp rename to planning/behavior_velocity_blind_spot_module/src/manager.cpp index a91d8ea32212c..14fc5ea2fa552 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "manager.hpp" + +#include +#include #include +#include #include #include #include @@ -82,3 +84,7 @@ BlindSpotModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp rename to planning/behavior_velocity_blind_spot_module/src/manager.hpp index dccb2616edd71..9aeaa0abfc4b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_ -#define SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -41,6 +44,11 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class BlindSpotModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp rename to planning/behavior_velocity_blind_spot_module/src/scene.cpp index 1e8795036bd0c..440c360e712a3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include +#include #include #include #include -#include #include -#include -#include -#include #include #include @@ -30,6 +31,7 @@ #include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp rename to planning/behavior_velocity_blind_spot_module/src/scene.hpp index 13810f503e42a..626879d3d3779 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__BLIND_SPOT__SCENE_HPP_ -#define SCENE_MODULE__BLIND_SPOT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ +#include +#include +#include #include -#include -#include -#include #include #include @@ -229,4 +229,4 @@ class BlindSpotModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__BLIND_SPOT__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt new file mode 100644 index 0000000000000..174488c81499f --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_crosswalk_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene_crosswalk.cpp + src/scene_walkway.cpp + src/util.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml new file mode 100644 index 0000000000000..e9b38f86a00ee --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -0,0 +1,45 @@ + + + + behavior_velocity_crosswalk_module + 0.1.0 + The behavior_velocity_crosswalk_module package + + Satoshi Ota + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pcl_conversions + pluginlib + rclcpp + route_handler + sensor_msgs + tier4_api_msgs + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml new file mode 100644 index 0000000000000..6ab5b00758f29 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp rename to planning/behavior_velocity_crosswalk_module/src/debug.cpp index 1b27e2b06cf55..0f902cdcf0848 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_crosswalk.hpp" +#include "scene_walkway.hpp" + +#include #include -#include -#include #include -#include #include @@ -222,7 +223,6 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr // stop_poses, const std::vector & slow_poses) { const auto now = this->clock_->now(); - auto id = module_id_; std::vector stop_poses; std::vector slow_down_poses; @@ -233,7 +233,7 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr } appendMarkerArray( virtual_wall_marker_creator_crosswalk_->createStopVirtualWallMarker( - stop_poses, "crosswalk", now, id), + stop_poses, "crosswalk", now, 0.0, std::to_string(module_id_) + "_"), &wall_marker); for (const auto & p : debug_data_.slow_poses) { const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); @@ -241,7 +241,7 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr } appendMarkerArray( virtual_wall_marker_creator_crosswalk_->createSlowDownVirtualWallMarker( - slow_down_poses, "crosswalk", now, id), + slow_down_poses, "crosswalk", now, 0.0, std::to_string(module_id_) + "_"), &wall_marker); return wall_marker; @@ -250,7 +250,6 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr visualization_msgs::msg::MarkerArray WalkwayModule::createVirtualWallMarkerArray() { const auto now = this->clock_->now(); - auto id = module_id_; std::vector stop_poses; visualization_msgs::msg::MarkerArray wall_marker; @@ -260,7 +259,7 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createVirtualWallMarkerArray } appendMarkerArray( virtual_wall_marker_creator_walkway_->createStopVirtualWallMarker( - stop_poses, "walkway", now, id), + stop_poses, "walkway", now, 0.0, std::to_string(module_id_) + "_"), &wall_marker); return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp rename to planning/behavior_velocity_crosswalk_module/src/manager.cpp index b60d0e87b6852..37d30e4e386bf 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include "manager.hpp" +#include + +#include #include #include #include @@ -195,3 +197,9 @@ WalkwayModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp similarity index 80% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp rename to planning/behavior_velocity_crosswalk_module/src/manager.hpp index 2c3f30cac048e..434c7eed9f065 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__CROSSWALK__MANAGER_HPP_ -#define SCENE_MODULE__CROSSWALK__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene_crosswalk.hpp" +#include "scene_walkway.hpp" + +#include +#include +#include #include -#include -#include -#include #include #include @@ -61,6 +64,15 @@ class WalkwayModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class CrosswalkModulePlugin : public PluginWrapper +{ +}; + +class WalkwayModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp rename to planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index a8d2391b4228d..6b2f15fe15719 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -12,14 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_crosswalk.hpp" + #include +#include +#include #include -#include #include -#include -#include +#include #include +#include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp rename to planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 2de34959ab5cb..72c03f62b207c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_ -#define SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_ +#ifndef SCENE_CROSSWALK_HPP_ +#define SCENE_CROSSWALK_HPP_ +#include "util.hpp" + +#include #include #include #include -#include -#include #include #include @@ -178,4 +179,4 @@ class CrosswalkModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_ +#endif // SCENE_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp rename to planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp index 7b968552ce654..ece8531e81f10 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_walkway.hpp" + +#include #include -#include -#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp similarity index 88% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp rename to planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp index ae06e478b9cfa..cb7dd9f47d303 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ -#define SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ +#ifndef SCENE_WALKWAY_HPP_ +#define SCENE_WALKWAY_HPP_ +#include "scene_crosswalk.hpp" +#include "util.hpp" + +#include #include #include -#include -#include -#include #include #include @@ -78,4 +79,4 @@ class WalkwayModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ +#endif // SCENE_WALKWAY_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp rename to planning/behavior_velocity_crosswalk_module/src/util.cpp index 385552ba75206..9a32f96838a92 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "util.hpp" + +#include #include -#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp b/planning/behavior_velocity_crosswalk_module/src/util.hpp similarity index 93% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp rename to planning/behavior_velocity_crosswalk_module/src/util.hpp index c99a815e577ee..56c97b4d9f91a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__CROSSWALK__UTIL_HPP_ -#define SCENE_MODULE__CROSSWALK__UTIL_HPP_ +#ifndef UTIL_HPP_ +#define UTIL_HPP_ #include #include @@ -27,10 +27,9 @@ #include #define EIGEN_MPL2_ONLY -#include "behavior_velocity_planner/planner_data.hpp" - #include #include +#include #include @@ -86,4 +85,4 @@ lanelet::Optional getStopLineFromMap( const std::string & attribute_name); } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__UTIL_HPP_ +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt new file mode 100644 index 0000000000000..5413517d9fb40 --- /dev/null +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_detection_area_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml new file mode 100644 index 0000000000000..66b8a62e83a2c --- /dev/null +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_velocity_detection_area_module + 0.1.0 + The behavior_velocity_detection_area_module package + + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kyoichi Sugahara + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml new file mode 100644 index 0000000000000..73497c8bfdf2a --- /dev/null +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp rename to planning/behavior_velocity_detection_area_module/src/debug.cpp index 9bd56c79cf70d..6316de449a44c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include #include -#include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -174,8 +175,6 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createVirtualWallMarke const rclcpp::Time now = clock_->now(); - auto id = getModuleId(); - std::vector stop_poses; std::vector dead_line_poses; @@ -185,8 +184,7 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createVirtualWallMarke stop_poses.push_back(p_front); } appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker( - stop_poses, "detection_area", now, id), + virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_poses, "detection_area", now), &wall_marker, now); for (const auto & p : debug_data_.dead_line_poses) { @@ -196,7 +194,7 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createVirtualWallMarke } appendMarkerArray( virtual_wall_marker_creator_->createDeadLineVirtualWallMarker( - dead_line_poses, "detection_area", now, id), + dead_line_poses, "detection_area", now), &wall_marker, now); return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp rename to planning/behavior_velocity_detection_area_module/src/manager.cpp index e09f46b627eeb..bde64c48f286d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "manager.hpp" + #include -#include #include +#include #include #include #include @@ -77,3 +79,7 @@ DetectionAreaModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp rename to planning/behavior_velocity_detection_area_module/src/manager.hpp index 0da425e208efe..10fca7182d09a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ -#define SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -41,6 +44,11 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class DetectionAreaModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_detection_area_module/src/scene.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp rename to planning/behavior_velocity_detection_area_module/src/scene.cpp index c50ac84d24a3d..53a6deb6f26a4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utilization/arc_lane_util.hpp" +#include "scene.hpp" +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -211,7 +211,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * std::pair calcSmallestEnclosingCircle( const lanelet::ConstPolygon2d & poly) { - // The `eps` is used to avoid precision bugs in circle inclusion checkings. + // The `eps` is used to avoid precision bugs in circle inclusion checks. // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is // recommended. const double eps = 1e-5; diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp rename to planning/behavior_velocity_detection_area_module/src/scene.hpp index 687fa92e920fc..75aa09dd41317 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ -#define SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include @@ -23,10 +23,10 @@ #define EIGEN_MPL2_ONLY #include +#include +#include #include #include -#include -#include #include #include @@ -106,4 +106,4 @@ class DetectionAreaModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt new file mode 100644 index 0000000000000..063430ad5d29e --- /dev/null +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_intersection_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene_intersection.cpp + src/scene_merge_from_private_road.cpp + src/util.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml new file mode 100644 index 0000000000000..25df9f2385d74 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -0,0 +1,53 @@ + + + + behavior_velocity_intersection_module + 0.1.0 + The behavior_velocity_intersection_module package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + cv_bridge + geometry_msgs + grid_map_cv + grid_map_ros + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + magic_enum + motion_utils + nav_msgs + pluginlib + rclcpp + route_handler + rtc_interface + tf2_geometry_msgs + tier4_api_msgs + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + grid_map_rviz_plugin + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_intersection_module/plugins.xml new file mode 100644 index 0000000000000..206f0324231ec --- /dev/null +++ b/planning/behavior_velocity_intersection_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp similarity index 72% rename from planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp rename to planning/behavior_velocity_intersection_module/src/debug.cpp index 394e542164b10..a5baa2ffa30ed 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_intersection.hpp" +#include "scene_merge_from_private_road.hpp" + +#include +#include #include -#include -#include -#include -#include #include #include @@ -103,6 +104,25 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } +visualization_msgs::msg::Marker createPointMarkerArray( + const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, + const double g, const double b) +{ + visualization_msgs::msg::Marker marker_point{}; + marker_point.header.frame_id = "map"; + marker_point.ns = ns + "_point"; + marker_point.id = id; + marker_point.lifetime = rclcpp::Duration::from_seconds(0.3); + marker_point.type = visualization_msgs::msg::Marker::SPHERE; + marker_point.action = visualization_msgs::msg::Marker::ADD; + marker_point.scale = createMarkerScale(2.0, 2.0, 2.0); + marker_point.color = createMarkerColor(r, g, b, 0.999); + + marker_point.pose.position = point; + + return marker_point; +} + } // namespace visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() @@ -143,6 +163,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( module_id_, now, 0.3, 0.0, 0.0, 0.5, 0.0, 0.0), &debug_marker_array, now); + if (!occlusion_safety_) { + debug_marker_array.markers.push_back(createPointMarkerArray( + debug_data_.nearest_occlusion_point, "nearest_occlusion", module_id_, 0.5, 0.5, 0.0)); + debug_marker_array.markers.push_back(createPointMarkerArray( + debug_data_.nearest_occlusion_projection_point, "nearest_occlusion_projection", module_id_, + 0.5, 0.5, 0.0)); + } + size_t i{0}; for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( @@ -176,12 +204,36 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker const auto now = this->clock_->now(); - if (debug_data_.stop_required) { + // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance + if (!activated_) { + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.collision_stop_wall_pose}, "intersection", now, 0.0, + "intersection" + std::to_string(module_id_) + "_"), + &wall_marker, now); + } + if (!activated_ && occlusion_first_stop_required_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.stop_wall_pose}, "intersection", now, module_id_), + {debug_data_.occlusion_first_stop_wall_pose}, "intersection", now, 0.0, + "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"), &wall_marker, now); } + if (!occlusion_activated_) { + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now, 0.0, + "intersection_occlusion" + std::to_string(module_id_) + "_"), + &wall_marker, now); + } + + auto id = planning_utils::bitShift(module_id_); + for (auto & marker : wall_marker.markers) { + if (marker.action == visualization_msgs::msg::Marker::ADD) { + marker.id = id; + id++; + } + } return wall_marker; } @@ -191,11 +243,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); + int32_t uid = planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray( - debug_data_.stop_point_pose, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), + createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } @@ -212,7 +264,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createVirtualWa const std::vector & pose = {debug_data_.virtual_wall_pose}; appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - pose, "merge_from_private_road", now, module_id_), + pose, "merge_from_private_road", now), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp similarity index 68% rename from planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp rename to planning/behavior_velocity_intersection_module/src/manager.cpp index 78a103a8878ec..6a1590b4197ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -12,22 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "manager.hpp" + +#include +#include #include -#include -#include -#include #include +#include #include #include #include +#include #include namespace behavior_velocity_planner { IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC(node, getModuleName()), + occlusion_rtc_interface_(&node, "intersection_occlusion") { const std::string ns(getModuleName()); auto & ip = intersection_param_; @@ -49,6 +53,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".common.intersection_max_accel"); ip.common.stop_overshoot_margin = node.declare_parameter(ns + ".common.stop_overshoot_margin"); + ip.common.use_intersection_area = + node.declare_parameter(ns + ".common.use_intersection_area"); ip.stuck_vehicle.use_stuck_stopline = node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); @@ -76,21 +82,24 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".collision_detection.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = node.declare_parameter(ns + ".collision_detection.keep_detection_vel_thr"); -} -MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()) -{ - const std::string ns(getModuleName()); - auto & mp = merge_from_private_area_param_; - mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); - mp.detection_area_length = - node.get_parameter("intersection.common.detection_area_length").as_double(); - mp.detection_area_right_margin = - node.get_parameter("intersection.common.detection_area_right_margin").as_double(); - mp.detection_area_left_margin = - node.get_parameter("intersection.common.detection_area_left_margin").as_double(); - mp.stop_line_margin = node.get_parameter("intersection.common.stop_line_margin").as_double(); + ip.occlusion.enable = node.declare_parameter(ns + ".occlusion.enable"); + ip.occlusion.occlusion_detection_area_length = + node.declare_parameter(ns + ".occlusion.occlusion_detection_area_length"); + ip.occlusion.enable_creeping = node.declare_parameter(ns + ".occlusion.enable_creeping"); + ip.occlusion.occlusion_creep_velocity = + node.declare_parameter(ns + ".occlusion.occlusion_creep_velocity"); + ip.occlusion.peeking_offset = node.declare_parameter(ns + ".occlusion.peeking_offset"); + ip.occlusion.free_space_max = node.declare_parameter(ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = node.declare_parameter(ns + ".occlusion.occupied_min"); + ip.occlusion.do_dp = node.declare_parameter(ns + ".occlusion.do_dp"); + ip.occlusion.before_creep_stop_time = + node.declare_parameter(ns + ".occlusion.before_creep_stop_time"); + ip.occlusion.min_vehicle_brake_for_rss = + node.declare_parameter(ns + ".occlusion.min_vehicle_brake_for_rss"); + ip.occlusion.max_vehicle_velocity_for_rss = + node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); + ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); } void IntersectionModuleManager::launchNewModules( @@ -101,6 +110,8 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + // run occlusion detection only in the first intersection + const bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -120,15 +131,129 @@ void IntersectionModuleManager::launchNewModules( const auto assoc_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); - registerModule(std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, assoc_ids, - logger_.get_child("intersection_module"), clock_)); + const auto new_module = std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection, + node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + /* set RTC status as non_occluded status initially */ + const UUID uuid = getUUID(new_module->getModuleId()); + const auto occlusion_uuid = new_module->getOcclusionUUID(); + rtc_interface_.updateCooperateStatus( + uuid, true, std::numeric_limits::lowest(), std::numeric_limits::lowest(), + clock_->now()); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + registerModule(std::move(new_module)); } } +std::function &)> +IntersectionModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = intersection_module->getAssocIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (assoc_ids.find(lane.id()) != assoc_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = intersection_module->getAssocIds(); + if (assoc_ids.find(lane.id()) != assoc_ids.end()) { + return true; + } + } + return false; +} + +void IntersectionModuleManager::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const UUID uuid = getUUID(scene_module->getModuleId()); + const bool safety = + scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired()); + updateRTCStatus(uuid, safety, scene_module->getDistance(), stamp); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + } + rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() + occlusion_rtc_interface_.publishCooperateStatus(stamp); +} + +void IntersectionModuleManager::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + } +} + +void IntersectionModuleManager::deleteExpiredModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + // default + removeRTCStatus(getUUID(scene_module->getModuleId())); + removeUUID(scene_module->getModuleId()); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); + unregisterModule(scene_module); + } + } +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + auto & mp = merge_from_private_area_param_; + mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + mp.detection_area_length = + node.get_parameter("intersection.common.detection_area_length").as_double(); + mp.detection_area_right_margin = + node.get_parameter("intersection.common.detection_area_right_margin").as_double(); + mp.detection_area_left_margin = + node.get_parameter("intersection.common.detection_area_left_margin").as_double(); + mp.stop_line_margin = node.get_parameter("intersection.common.stop_line_margin").as_double(); +} + void MergeFromPrivateModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { @@ -195,32 +320,6 @@ void MergeFromPrivateModuleManager::launchNewModules( } } -std::function &)> -IntersectionModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) -{ - const auto lane_set = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - - return [this, lane_set](const std::shared_ptr & scene_module) { - const auto intersection_module = std::dynamic_pointer_cast(scene_module); - const auto & assoc_ids = intersection_module->getAssocIds(); - for (const auto & lane : lane_set) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - const auto is_intersection = - turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; - if (!is_intersection) { - continue; - } - - if (assoc_ids.find(lane.id()) != assoc_ids.end() /* contains */) { - return false; - } - } - return true; - }; -} - std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) @@ -248,19 +347,6 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( }; } -bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( - const lanelet::ConstLanelet & lane) const -{ - for (const auto & scene_module : scene_modules_) { - const auto intersection_module = std::dynamic_pointer_cast(scene_module); - const auto & assoc_ids = intersection_module->getAssocIds(); - if (assoc_ids.find(lane.id()) != assoc_ids.end()) { - return true; - } - } - return false; -} - bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( const lanelet::ConstLanelet & lane) const { @@ -276,3 +362,10 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::MergeFromPrivateModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp similarity index 68% rename from planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp rename to planning/behavior_velocity_intersection_module/src/manager.hpp index 7e22a2be65d71..ff9302db0b6af 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__MANAGER_HPP_ -#define SCENE_MODULE__INTERSECTION__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene_intersection.hpp" +#include "scene_merge_from_private_road.hpp" + +#include +#include +#include #include -#include -#include -#include #include #include @@ -26,6 +29,7 @@ #include #include #include +#include namespace behavior_velocity_planner { @@ -38,6 +42,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC private: IntersectionModule::PlannerParam intersection_param_; + // additional for INTERSECTION_OCCLUSION + RTCInterface occlusion_rtc_interface_; + std::unordered_map occlusion_map_uuid_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; @@ -45,6 +52,12 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; + + /* called from SceneModuleInterfaceWithRTC::plan */ + void sendRTC(const Time & stamp) override; + void setActivation() override; + /* called from SceneModuleInterface::updateSceneModuleInstances */ + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -64,6 +77,15 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; + +class IntersectionModulePlugin : public PluginWrapper +{ +}; + +class MergeFromPrivateModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp similarity index 55% rename from planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp rename to planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 9009f507883fb..e0a602b5f605a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -12,22 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include + +#if __has_include() +#include +#else +#include +#endif +// #include +// #include +#include "scene_intersection.hpp" +#include "util.hpp" + +#include +#include +#include +#include +#include +#include #include #include #include +#include #include +#include #include namespace behavior_velocity_planner @@ -53,12 +68,16 @@ static geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const std::set & assoc_ids, const rclcpp::Logger logger, + const PlannerParam & planner_param, const std::set & assoc_ids, + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + node_(node), lane_id_(lane_id), - is_go_out_(false), - assoc_ids_(assoc_ids) + assoc_ids_(assoc_ids), + enable_occlusion_detection_(enable_occlusion_detection), + detection_divisions_(std::nullopt), + occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; @@ -66,9 +85,15 @@ IntersectionModule::IntersectionModule( const auto & assigned_lanelet = planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); - has_traffic_light_ = - !(assigned_lanelet.regulatoryElementsAs().empty()); - state_machine_.setMarginTime(planner_param_.collision_detection.state_transit_margin_time); + collision_state_machine_.setMarginTime( + planner_param_.collision_detection.state_transit_margin_time); + before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + // TODO(Mamoru Sobue): maybe optional is better + before_creep_state_machine_.setState(StateMachine::State::STOP); + if (enable_occlusion_detection) { + occlusion_grid_pub_ = node_.create_publisher( + "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local()); + } } bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -76,11 +101,16 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * RCLCPP_DEBUG(logger_, "===== plan start ====="); debug_data_ = DebugData(); - const StateMachine::State current_state = state_machine_.getState(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - RCLCPP_DEBUG( - logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); + /* set default RTC */ + // safe_, distance_ + setSafe(true); + setDistance(std::numeric_limits::lowest()); + // occlusion + occlusion_safety_ = true; + occlusion_stop_distance_ = std::numeric_limits::lowest(); + occlusion_first_stop_required_ = false; /* get current pose */ const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; @@ -98,16 +128,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (!splineInterpolate(*path, interval, path_ip, logger_)) { RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } const auto lane_interval_ip_opt = util::findLaneIdsInterval(path_ip, assoc_ids_); if (!lane_interval_ip_opt) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } const auto lane_interval_ip = lane_interval_ip_opt.value(); @@ -119,40 +145,42 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if ( !intersection_lanelets_ || intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) { + const auto lanelets_on_path = planning_utils::getLaneletsOnPath( + *path, lanelet_map_ptr, planner_data_->current_odometry->pose); intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, assoc_ids_, path_ip, lane_interval_ip, - planner_param_.common.detection_area_length, tl_arrow_solid_on); + lanelet_map_ptr, routing_graph_ptr, lane_id_, lanelets_on_path, assoc_ids_, path_ip, + lane_interval_ip, planner_param_.common.detection_area_length, tl_arrow_solid_on); } const auto & detection_lanelets = intersection_lanelets_.value().attention; const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent; + const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention; const auto & detection_area = intersection_lanelets_.value().attention_area; + const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area; const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area; const auto & first_detection_area = intersection_lanelets_.value().first_detection_area; + const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting; debug_data_.detection_area = detection_area; debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area; /* get intersection area */ - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + const auto intersection_area = planner_param_.common.use_intersection_area + ? util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr) + : std::nullopt; if (intersection_area) { const auto intersection_area_2d = intersection_area.value(); debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - const std::optional stuck_line_idx_opt = - first_conflicting_area - ? util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin, - planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval, - lane_interval_ip, logger_.get_child("util")) - : std::nullopt; + if (conflicting_lanelets.empty()) { + RCLCPP_DEBUG(logger_, "conflicting area is empty"); + return false; + } - /* set stop lines for base_link */ - const auto stop_lines_idx_opt = first_detection_area - ? util::generateStopLine( - lane_id_, first_detection_area.value(), planner_data_, - planner_param_.common.stop_line_margin, path, path_ip, - interval, lane_interval_ip, logger_.get_child("util")) - : std::nullopt; + if (!detection_divisions_.has_value()) { + detection_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_lanelets, routing_graph_ptr, + planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + } /* calc closest index */ const auto closest_idx_opt = @@ -161,68 +189,192 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "motion_utils::findNearestIndex fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } const size_t closest_idx = closest_idx_opt.get(); - if (stop_lines_idx_opt) { - const auto stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; - const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; + const auto static_pass_judge_line_opt = + first_detection_area + ? util::generateStaticPassJudgeLine( + first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_) + : std::nullopt; + if (static_pass_judge_line_opt) { + const auto pass_judge_line_idx = static_pass_judge_line_opt.value(); const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - - /* if ego is over the pass judge line before collision is detected, keep going */ - const double current_velocity = planner_data_->current_velocity->twist.linear.x; - if ( - is_over_pass_judge_line && is_go_out_ && - current_velocity > planner_param_.collision_detection.keep_detection_vel_thr) { + const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); + const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + // if ego is over the pass judge line and not stopped + if (is_over_pass_judge_line && is_go_out_ && !keep_detection) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx).point.pose.position)); return true; } } - /* collision checking */ - bool is_entry_prohibited = false; - - /* get dynamic object */ - const auto objects_ptr = planner_data_->predicted_objects; - /* considering lane change in the intersection, these lanelets are generated from the path */ const auto ego_lane_with_next_lane = getEgoLaneWithNextLane(*path, planner_data_->vehicle_info_.vehicle_width_m); const auto ego_lane = ego_lane_with_next_lane.front(); debug_data_.ego_lane = ego_lane.polygon3d(); + /* get dynamic object */ + // TODO(Mamoru Sobue): filter objects on detection area here + const auto objects_ptr = planner_data_->predicted_objects; + /* check stuck vehicle */ const auto stuck_vehicle_detect_area = generateStuckVehicleDetectAreaPolygon(*path, ego_lane_with_next_lane, closest_idx); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + const std::optional stuck_line_idx_opt = + first_conflicting_area + ? util::generateStuckStopLine( + first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin, + planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval, + lane_interval_ip, logger_.get_child("util")) + : std::nullopt; /* calculate dynamic collision around detection area */ - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + /* set stop lines for base_link */ + const auto default_stop_line_idx_opt = + first_detection_area ? util::generateCollisionStopLine( + lane_id_, first_detection_area.value(), planner_data_, + planner_param_.common.stop_line_margin, path, path_ip, interval, + lane_interval_ip, logger_.get_child("util")) + : std::nullopt; const double time_delay = is_go_out_ ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - - state_machine_.getDuration()); + collision_state_machine_.getDuration()); const bool has_collision = checkCollision( lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, ego_lane, ego_lane_with_next_lane, objects_ptr, closest_idx, time_delay); + /* check occlusion on detection lane */ + const double occlusion_dist_thr = std::fabs( + std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / + (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); + const bool is_occlusion_cleared = + (enable_occlusion_detection_ && first_detection_area && !occlusion_attention_lanelets.empty()) + ? isOcclusionCleared( + *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, + first_detection_area.value(), path_ip, lane_interval_ip, detection_divisions_.value(), + occlusion_dist_thr) + : true; + const auto occlusion_peeking_line_idx_opt = + first_detection_area + ? util::generatePeekingLimitLine( + first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_, + planner_param_.occlusion.peeking_offset) + : std::nullopt; + /* calculate final stop lines */ - std::optional stop_line_idx = - stop_lines_idx_opt ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) - : std::nullopt; - if (is_stuck && stuck_line_idx_opt) { - is_entry_prohibited = true; + std::optional stop_line_idx = default_stop_line_idx_opt; + std::optional occlusion_peeking_line_idx = + occlusion_peeking_line_idx_opt + ? std::make_optional(occlusion_peeking_line_idx_opt.value()) + : std::nullopt; + std::optional occlusion_first_stop_line_idx = default_stop_line_idx_opt; + std::optional> insert_creep_during_occlusion = std::nullopt; + + /* set RTC distance */ + const double dist_1st_stopline = + default_stop_line_idx_opt + ? motion_utils::calcSignedArcLength( + path_ip.points, current_pose.position, + path->points.at(default_stop_line_idx_opt.value()).point.pose.position) + : std::numeric_limits::lowest(); + const double dist_2nd_stopline = + occlusion_peeking_line_idx + ? motion_utils::calcSignedArcLength( + path_ip.points, current_pose.position, + path->points.at(occlusion_peeking_line_idx.value()).point.pose.position) + : std::numeric_limits::lowest(); + + bool stuck_stop_required = false; + bool collision_stop_required = false; + bool first_phase_stop_required = false; + bool occlusion_stop_required = false; + + /* check safety */ + const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); + is_actually_occluded_ = !is_occlusion_cleared; + is_forcefully_occluded_ = ext_occlusion_requested; + if (!is_occlusion_cleared || ext_occlusion_requested) { + const bool approached_stop_line = + (std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(); + if (!default_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "occlusion is detected but default stop line is not set or generated"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; + } else if (before_creep_state_machine_.getState() == StateMachine::State::GO) { + if (!has_collision) { + occlusion_stop_required = true; + occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt; + // clear first stop line + // insert creep velocity [closest_idx, occlusion_stop_line) + insert_creep_during_occlusion = + std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value()); + occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; + } else { + collision_stop_required = true; + stop_line_idx = default_stop_line_idx_opt; + occlusion_stop_required = true; + occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt; + // clear first stop line + // insert creep velocity [closest_idx, occlusion_stop_line) + insert_creep_during_occlusion = + std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value()); + occlusion_state_ = OcclusionState::COLLISION_DETECTED; + } + } else { + if (is_stopped && approached_stop_line) { + // start waiting at the first stop line + before_creep_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_); + occlusion_state_ = OcclusionState::WAIT_FIRST_STOP_LINE; + } + first_phase_stop_required = true; + occlusion_stop_required = true; + occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt; + stop_line_idx = occlusion_first_stop_line_idx; + // insert creep velocity [default_stop_line, occlusion_stop_line) + insert_creep_during_occlusion = + default_stop_line_idx_opt && occlusion_peeking_line_idx_opt + ? std::make_optional>( + default_stop_line_idx_opt.value(), occlusion_peeking_line_idx_opt.value()) + : std::nullopt; + occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE; + } + } else if (occlusion_state_ != OcclusionState::CLEARED) { + // previously occlusion existed, but now it is clear + if ( + default_stop_line_idx_opt && + !util::isOverTargetIndex( + *path, closest_idx, current_pose, default_stop_line_idx_opt.value())) { + stop_line_idx = default_stop_line_idx_opt.value(); + } else if ( + static_pass_judge_line_opt && + !util::isOverTargetIndex( + *path, closest_idx, current_pose, static_pass_judge_line_opt.value())) { + stop_line_idx = static_pass_judge_line_opt; + } + occlusion_state_ = OcclusionState::CLEARED; + if (stop_line_idx && has_collision) { + // do collision checking at previous occlusion stop line + collision_stop_required = true; + } else { + collision_stop_required = false; + } + if (is_stuck && stuck_line_idx_opt) { + stuck_stop_required = true; + stop_line_idx = static_pass_judge_line_opt; + } + } else if (is_stuck && stuck_line_idx_opt) { + stuck_stop_required = true; const size_t stuck_line_idx = stuck_line_idx_opt.value(); const double dist_stuck_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(stuck_line_idx).point.pose.position, @@ -232,40 +384,74 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); if (!is_over_stuck_stopline) { stop_line_idx = stuck_line_idx; - } else if (is_over_stuck_stopline && stop_lines_idx_opt) { - stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; + } else if (is_over_stuck_stopline && default_stop_line_idx_opt) { + stop_line_idx = default_stop_line_idx_opt.value(); } } else if (has_collision) { - is_entry_prohibited = true; - stop_line_idx = stop_lines_idx_opt - ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) - : std::nullopt; + collision_stop_required = true; + stop_line_idx = default_stop_line_idx_opt; } - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); - setSafe(state_machine_.getState() == StateMachine::State::GO); - if (!stop_line_idx) { - RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); + RCLCPP_DEBUG(logger_, "detection_area is empty"); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx.value()).point.pose.position)); + const std::string occlusion_state = std::string(magic_enum::enum_name(occlusion_state_)); + RCLCPP_DEBUG(logger_, "occlusion state: OcclusionState::%s", occlusion_state.c_str()); + + /* for collision and stuck state */ + collision_state_machine_.setStateWithMarginTime( + (collision_stop_required || stuck_stop_required) ? StateMachine::State::STOP + : StateMachine::State::GO, + logger_.get_child("collision state_machine"), *clock_); + + /* set RTC safety respectively */ + occlusion_stop_distance_ = dist_2nd_stopline; + setDistance(dist_1st_stopline); + if (occlusion_stop_required) { + occlusion_first_stop_required_ = first_phase_stop_required; + occlusion_safety_ = is_occlusion_cleared; + } + /* collision */ + setSafe(collision_state_machine_.getState() == StateMachine::State::GO); + + /* make decision */ + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + if (!occlusion_activated_) { + is_go_out_ = false; + + /* in case of creeping */ + if (insert_creep_during_occlusion && planner_param_.occlusion.enable_creeping) { + const auto [start, end] = insert_creep_during_occlusion.value(); + for (size_t i = start; i < end; ++i) { + planning_utils::setVelocityFromIndex( + i, planner_param_.occlusion.occlusion_creep_velocity /* [m/s] */, path); + } + } + + if (!isActivated() && occlusion_first_stop_required_ && occlusion_first_stop_line_idx) { + planning_utils::setVelocityFromIndex( + occlusion_first_stop_line_idx.value(), 0.0 /* [m/s] */, path); + debug_data_.occlusion_first_stop_wall_pose = + planning_utils::getAheadPose(occlusion_first_stop_line_idx.value(), baselink2front, *path); + } + + if (occlusion_peeking_line_idx) { + planning_utils::setVelocityFromIndex( + occlusion_peeking_line_idx.value(), 0.0 /* [m/s] */, path); + debug_data_.occlusion_stop_wall_pose = + planning_utils::getAheadPose(occlusion_peeking_line_idx.value(), baselink2front, *path); + } + RCLCPP_DEBUG(logger_, "===== plan end ====="); + } - if (!isActivated()) { - // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block + if (!isActivated() /* collision*/) { is_go_out_ = false; planning_utils::setVelocityFromIndex(stop_line_idx.value(), 0.0 /* [m/s] */, path); - debug_data_.stop_required = true; // dangerous or disabled by RTC - debug_data_.stop_wall_pose = + debug_data_.collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx.value(), baselink2front, *path); // Get stop point and stop factor @@ -283,8 +469,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } - - RCLCPP_DEBUG(logger_, "not activated. stop at the line."); RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } @@ -780,4 +964,294 @@ bool IntersectionModule::checkFrontVehicleDeceleration( return false; } +bool IntersectionModule::isOcclusionCleared( + const nav_msgs::msg::OccupancyGrid & occ_grid, + const std::vector & detection_areas, + lanelet::ConstLanelets adjacent_lanelets, const lanelet::CompoundPolygon3d & first_detection_area, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, + const std::pair & lane_interval_ip, + const std::vector & lane_divisions, + const double occlusion_dist_thr) const +{ + const auto first_detection_area_idx = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); + if (!first_detection_area_idx) { + return false; + } + + const auto first_inside_detection_idx_ip_opt = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); + const std::pair lane_detection_interval_ip = + first_inside_detection_idx_ip_opt + ? std::make_pair(first_inside_detection_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) + : lane_interval_ip; + + const int width = occ_grid.info.width; + const int height = occ_grid.info.height; + const double resolution = occ_grid.info.resolution; + const auto & origin = occ_grid.info.origin.position; + + // NOTE: interesting area is set to 0 for later masking + cv::Mat detection_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); + + // (1) prepare detection area mask + Polygon2d grid_poly; + grid_poly.outer().emplace_back(origin.x, origin.y); + grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); + grid_poly.outer().emplace_back( + origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y); + bg::correct(grid_poly); + + std::vector> detection_area_cv_polygons; + for (const auto & detection_area : detection_areas) { + const auto area2d = lanelet::utils::to2D(detection_area); + Polygon2d area2d_poly; + for (const auto & p : area2d) { + area2d_poly.outer().emplace_back(p.x(), p.y()); + } + area2d_poly.outer().push_back(area2d_poly.outer().front()); + bg::correct(area2d_poly); + std::vector common_areas; + bg::intersection(area2d_poly, grid_poly, common_areas); + if (common_areas.empty()) { + continue; + } + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + for (const auto & common_area : common_areas) { + std::vector detection_area_cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + detection_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + detection_area_cv_polygons.push_back(detection_area_cv_polygon); + } + } + for (const auto & poly : detection_area_cv_polygons) { + cv::fillPoly(detection_mask, poly, cv::Scalar(255), cv::LINE_AA); + } + // (1.1) + // reset adjacent_lanelets area to 0 on detection_mask + std::vector> adjacent_lane_cv_polygons; + for (const auto & adjacent_lanelet : adjacent_lanelets) { + const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); + Polygon2d area2d_poly; + for (const auto & p : area2d) { + area2d_poly.outer().emplace_back(p.x(), p.y()); + } + area2d_poly.outer().push_back(area2d_poly.outer().front()); + bg::correct(area2d_poly); + std::vector common_areas; + bg::intersection(area2d_poly, grid_poly, common_areas); + if (common_areas.empty()) { + continue; + } + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + for (const auto & common_area : common_areas) { + std::vector adjacent_lane_cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon); + } + } + for (const auto & poly : adjacent_lane_cv_polygons) { + cv::fillPoly(detection_mask, poly, cv::Scalar(0), cv::LINE_AA); + } + + // (2) prepare unknown mask + // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + const int idx = y * width + x; + const unsigned char intensity = occ_grid.data.at(idx); + if ( + planner_param_.occlusion.free_space_max <= intensity && + intensity < planner_param_.occlusion.occupied_min) { + unknown_mask.at(height - 1 - y, x) = 255; + } + } + } + + // (3) occlusion mask + cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(detection_mask, unknown_mask, occlusion_mask_raw); + // (3.1) apply morphologyEx + cv::Mat occlusion_mask; + const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / resolution); + cv::morphologyEx( + occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); + + // (4) create distance grid + // value: 0 - 254: signed distance representing [distance_min, distance_max] + // 255: undefined value + const double distance_max = std::hypot(width * resolution / 2, height * resolution / 2); + const double distance_min = -distance_max; + const int undef_pixel = 255; + const int max_cost_pixel = 254; + + auto dist2pixel = [=](const double dist) { + return std::min( + max_cost_pixel, + static_cast((dist - distance_min) / (distance_max - distance_min) * max_cost_pixel)); + }; + auto pixel2dist = [=](const int pixel) { + return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; + }; + + const int zero_dist_pixel = dist2pixel(0.0); + + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); + cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); + + const auto [lane_start, lane_end] = lane_detection_interval_ip; + for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { + const auto & path_pos = path_ip.points.at(i).point.pose.position; + const int idx_x = (path_pos.x - origin.x) / resolution; + const int idx_y = (path_pos.y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) continue; + if (idx_y < 0 || idx_y >= height) continue; + distance_grid.at(height - 1 - idx_y, idx_x) = zero_dist_pixel; + projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; + } + + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + for (const auto & division : divisions) { + bool is_in_grid = false; + bool zero_dist_cell_found = false; + int projection_ind = -1; + std::optional> cost_prev_checkpoint = + std::nullopt; // cost, x, y, projection_ind + for (const auto & point : division) { + const auto [valid, idx_x, idx_y] = coord2index(point.x(), point.y()); + // exited grid just now + if (is_in_grid && !valid) break; + + // still not entering grid + if (!is_in_grid && !valid) continue; + + // From here, "valid" + const int pixel = distance_grid.at(height - 1 - idx_y, idx_x); + + // entered grid for 1st time + if (!is_in_grid) { + assert(pixel == undef_pixel || pixel == zero_dist_pixel); + is_in_grid = true; + if (pixel == undef_pixel) { + continue; + } + } + + if (pixel == zero_dist_pixel) { + zero_dist_cell_found = true; + projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); + assert(projection_ind >= 0); + cost_prev_checkpoint = std::make_optional>( + 0.0, point.x(), point.y(), projection_ind); + continue; + } + + if (zero_dist_cell_found) { + // finally traversed to defined cell (first half) + const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = + cost_prev_checkpoint.value(); + const double dy = point.y() - prev_checkpoint_y, dx = point.x() - prev_checkpoint_x; + double new_dist = prev_cost + std::hypot(dy, dx); + const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); + const double cur_dist = pixel2dist(pixel); + if (planner_param_.occlusion.do_dp && cur_dist < new_dist) { + new_dist = cur_dist; + if (new_projection_ind > 0) { + projection_ind = std::min(prev_projection_ind, new_projection_ind); + } + } + projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; + distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); + cost_prev_checkpoint = std::make_optional>( + new_dist, point.x(), point.y(), projection_ind); + } + } + } + } + + // clean-up and find nearest risk + const int min_cost_thr = dist2pixel(occlusion_dist_thr); + int min_cost = undef_pixel - 1; + int max_cost = 0; + std::optional min_cost_projection_ind = std::nullopt; + geometry_msgs::msg::Point nearest_occlusion_point; + for (int i = 0; i < width; ++i) { + for (int j = 0; j < height; ++j) { + const int pixel = static_cast(distance_grid.at(height - 1 - j, i)); + const bool occluded = (occlusion_mask.at(height - 1 - j, i) == 255); + if (pixel == undef_pixel || !occluded) { + // ignore outside of cropped + // some cell maybe undef still + distance_grid.at(height - 1 - j, i) = 0; + continue; + } + if (max_cost < pixel) { + max_cost = pixel; + } + const int projection_ind = projection_ind_grid.at(height - 1 - j, i); + if (pixel < min_cost) { + assert(projection_ind >= 0); + min_cost = pixel; + min_cost_projection_ind = projection_ind; + nearest_occlusion_point.x = origin.x + i * resolution; + nearest_occlusion_point.y = origin.y + j * resolution; + nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel; + } + } + } + debug_data_.nearest_occlusion_point = nearest_occlusion_point; + + cv::Mat distance_grid_heatmap; + cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET); + /* + cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL); + cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap); + cv::waitKey(1); + */ + grid_map::GridMap occlusion_grid({"elevation"}); + occlusion_grid.setFrameId("map"); + occlusion_grid.setGeometry( + grid_map::Length(width * resolution, height * resolution), resolution, + grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2)); + cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE); + cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE); + grid_map::GridMapCvConverter::addLayerFromImage( + distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */, + origin.z + distance_max /* elevation for 255 */); + grid_map::GridMapCvConverter::addColorLayerFromImage( + distance_grid_heatmap, "color", occlusion_grid); + occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid)); + if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) { + return true; + } else { + return false; + } +} + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp similarity index 76% rename from planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp rename to planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6d96bd204a9ce..0a1697d4a832d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -12,21 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ -#define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ +#ifndef SCENE_INTERSECTION_HPP_ +#define SCENE_INTERSECTION_HPP_ +#include "util_type.hpp" + +#include +#include +#include +#include #include #include -#include -#include #include -#include -#include #include #include #include #include +#include #include #include @@ -48,9 +51,9 @@ class IntersectionModule : public SceneModuleInterface public: struct DebugData { - bool stop_required = false; - - geometry_msgs::msg::Pose stop_wall_pose; + geometry_msgs::msg::Pose collision_stop_wall_pose; + geometry_msgs::msg::Pose occlusion_stop_wall_pose; + geometry_msgs::msg::Pose occlusion_first_stop_wall_pose; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; @@ -62,6 +65,8 @@ class IntersectionModule : public SceneModuleInterface autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; geometry_msgs::msg::Pose predicted_obj_pose; + geometry_msgs::msg::Point nearest_occlusion_point; + geometry_msgs::msg::Point nearest_occlusion_projection_point; }; public: @@ -80,6 +85,7 @@ class IntersectionModule : public SceneModuleInterface double intersection_velocity; //! used for intersection passing time double intersection_max_acc; //! used for calculating intersection velocity double stop_overshoot_margin; //! overshoot margin for stuck, collision detection + bool use_intersection_area; } common; struct StuckVehicle { @@ -102,12 +108,37 @@ class IntersectionModule : public SceneModuleInterface double collision_end_margin_time; //! end margin time to check collision double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr } collision_detection; + struct Occlusion + { + bool enable; + double occlusion_detection_area_length; //! used for occlusion detection + bool enable_creeping; + double occlusion_creep_velocity; //! the creep velocity to occlusion limit stop line + double peeking_offset; + int free_space_max; + int occupied_min; + bool do_dp; + double before_creep_stop_time; + double min_vehicle_brake_for_rss; + double max_vehicle_velocity_for_rss; + double denoise_kernel; + } occlusion; + }; + + enum OcclusionState { + NONE, + BEFORE_FIRST_STOP_LINE, + WAIT_FIRST_STOP_LINE, + CREEP_SECOND_STOP_LINE, + CLEARED, + COLLISION_DETECTED, }; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & assoc_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -120,11 +151,18 @@ class IntersectionModule : public SceneModuleInterface const std::set & getAssocIds() const { return assoc_ids_; } + UUID getOcclusionUUID() const { return occlusion_uuid_; } + bool getOcclusionSafety() const { return occlusion_safety_; } + double getOcclusionDistance() const { return occlusion_stop_distance_; } + void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } + bool isOccluded() const { return is_actually_occluded_ || is_forcefully_occluded_; } + bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; } + private: + rclcpp::Node & node_; const int64_t lane_id_; std::string turn_direction_; - bool has_traffic_light_; - bool is_go_out_; + bool is_go_out_ = false; // Parameter PlannerParam planner_param_; std::optional intersection_lanelets_; @@ -132,6 +170,25 @@ class IntersectionModule : public SceneModuleInterface // have same turn_direction const std::set assoc_ids_; + // for occlusion detection + const bool enable_occlusion_detection_; + std::optional> detection_divisions_; + bool is_actually_occluded_ = false; //! occlusion based on occupancy_grid + bool is_forcefully_occluded_ = false; //! fake occlusion forced by external operator + OcclusionState occlusion_state_ = OcclusionState::NONE; + // NOTE: uuid_ is base member + // for occlusion clearance decision + const UUID occlusion_uuid_; + bool occlusion_safety_ = true; + double occlusion_stop_distance_; + bool occlusion_activated_ = true; + // for first stop in two-phase stop + const UUID occlusion_first_stop_uuid_; + bool occlusion_first_stop_required_ = false; + + StateMachine collision_state_machine_; //! for stable collision checking + StateMachine before_creep_state_machine_; //! for two phase stop + /** * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as * actual collision check algorithm inside this function) @@ -258,14 +315,24 @@ class IntersectionModule : public SceneModuleInterface lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - StateMachine state_machine_; //! for state + + bool isOcclusionCleared( + const nav_msgs::msg::OccupancyGrid & occ_grid, + const std::vector & detection_areas, + lanelet::ConstLanelets adjacent_lanelets, + const lanelet::CompoundPolygon3d & first_detection_area, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, + const std::pair & lane_interval, + const std::vector & lane_divisions, + const double occlusion_dist_thr) const; // Debug mutable DebugData debug_data_; std::shared_ptr virtual_wall_marker_creator_ = std::make_shared(); + rclcpp::Publisher::SharedPtr occlusion_grid_pub_; }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ +#endif // SCENE_INTERSECTION_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp similarity index 92% rename from planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp rename to planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 8297e95c1939e..9e2a3b9535c2a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -12,18 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_merge_from_private_road.hpp" + +#include "util.hpp" + +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include #include #include +#include #include #include #include @@ -86,25 +89,25 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR /* get detection area */ if (!intersection_lanelets_.has_value()) { intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, {} /* not used here */, path_ip, + lanelet_map_ptr, routing_graph_ptr, lane_id_, {}, {} /* not used here */, path_ip, lane_interval_ip, planner_param_.detection_area_length, false /* tl_arrow_solid on does not matter here*/); } const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area; /* set stop-line and stop-judgement-line for base_link */ - const auto stop_lines_idx_opt = + const auto stop_line_idx_opt = first_conflicting_area - ? util::generateStopLine( + ? util::generateCollisionStopLine( lane_id_, first_conflicting_area.value(), planner_data_, planner_param_.stop_line_margin, path, path_ip, interval, lane_interval_ip, logger_.get_child("util")) : std::nullopt; - if (!stop_lines_idx_opt.has_value()) { + if (!stop_line_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - const size_t stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; + const size_t stop_line_idx = stop_line_idx_opt.value(); if (stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp similarity index 89% rename from planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp rename to planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index c631a96c127b8..cea85e3b2daf2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#define SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#include "scene_intersection.hpp" + +#include +#include +#include #include -#include -#include #include -#include -#include #include #include @@ -109,4 +110,4 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#endif // SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp similarity index 69% rename from planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp rename to planning/behavior_velocity_intersection_module/src/util.cpp index d6440d931717d..4270427821dd1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -12,22 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "util.hpp" + +#include +#include #include #include #include #include #include -#include -#include -#include + +#include #include #include #include +#include #include #include #include +#include +#include #include namespace behavior_velocity_planner @@ -123,40 +129,108 @@ std::optional getDuplicatedPointIdx( return std::nullopt; } -std::optional> getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +std::optional generateStaticPassJudgeLine( + const lanelet::CompoundPolygon3d & first_detection_area, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, const std::pair lane_interval, - const std::vector & polygons) + const std::shared_ptr & planner_data) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>(i, polygon); - } + const auto pass_judge_line_idx_ip = + util::getFirstPointInsidePolygon(path_ip, lane_interval, first_detection_area); + if (!pass_judge_line_idx_ip) { + return std::nullopt; + } + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.vehicle_length_m / ip_interval); + const int idx = static_cast(pass_judge_line_idx_ip.value()) - base2front_idx_dist; + if (idx < 0) { + return std::nullopt; + } + const auto & insert_point = path_ip.points.at(static_cast(idx)).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt) { + return duplicate_idx_opt; + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt) { + return std::nullopt; } - if (is_in_lanelet) { + return insert_idx_opt; + } +} + +std::optional generatePeekingLimitLine( + const lanelet::CompoundPolygon3d & first_detection_area, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, + const std::shared_ptr & planner_data, const double offset) +{ + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); + std::optional first_collision = std::nullopt; + for (size_t i = 0; i <= std::get<1>(lane_interval); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + first_collision = i; break; } } - return std::nullopt; + if (!first_collision || first_collision.value() == 0) { + return std::nullopt; + } + + const int idx = first_collision.value() - 1 + std::ceil(offset / ip_interval); + if (idx < 0) { + return std::nullopt; + } + + const auto & insert_point = path_ip.points.at(static_cast(idx)).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt) { + return duplicate_idx_opt; + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt) { + return std::nullopt; + } + return insert_idx_opt; + } } -static std::optional getFirstPointInsidePolygon( +std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) { + const auto polygon_2d = lanelet::utils::to2D(polygon); for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; auto p = path.points.at(i).point.pose.position; - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional(i); } + } + return std::nullopt; +} + +std::optional> getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons) +{ + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + auto p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>(i, polygon); + } + } if (is_in_lanelet) { break; } @@ -164,29 +238,16 @@ static std::optional getFirstPointInsidePolygon( return std::nullopt; } -std::optional generateStopLine( +std::optional generateCollisionStopLine( const int lane_id, const lanelet::CompoundPolygon3d & detection_area, const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, const std::pair lane_interval_ip, const rclcpp::Logger logger) { - /* set judge line dist */ - const double current_vel = planner_data->current_velocity->twist.linear.x; - const double current_acc = planner_data->current_acceleration->accel.accel.linear.x; - const double max_acc = planner_data->max_stop_acceleration_threshold; - const double max_jerk = planner_data->max_stop_jerk_threshold; - const double delay_response_time = planner_data->delay_response_time; - const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - current_vel, current_acc, max_acc, max_jerk, delay_response_time); - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); - const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); - - /* generate stop points */ - util::StopLineIdx idxs; // If a stop_line tag is defined on lanelet_map, use it. // else generate a stop_line behind the intersection of path and detection area @@ -218,59 +279,25 @@ std::optional generateStopLine( return std::nullopt; } - { - /* insert stop_point */ - const auto & insert_point = path_ip.points.at(stop_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt) { - idxs.collision_stop_line = duplicate_idx_opt.value(); - } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt) { - RCLCPP_WARN(logger, "insertPoint failed for stop line"); - return std::nullopt; - } - idxs.collision_stop_line = insert_idx_opt.value(); - } - } - - const bool has_prior_stopline = std::any_of( - original_path->points.begin(), original_path->points.begin() + idxs.collision_stop_line, - [](const auto & p) { return std::fabs(p.point.longitudinal_velocity_mps) < 0.1; }); - - /* insert judge point */ - const size_t pass_judge_idx_ip = static_cast(std::min( - static_cast(path_ip.points.size()) - 1, - std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { - idxs.pass_judge_line = idxs.collision_stop_line; + /* insert stop_point */ + std::optional collision_stop_line = std::nullopt; + const auto & insert_point = path_ip.points.at(stop_idx_ip).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt) { + collision_stop_line = duplicate_idx_opt.value(); } else { - const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt) { - idxs.pass_judge_line = duplicate_idx_opt.value(); + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt) { + RCLCPP_WARN(logger, "insertPoint failed for stop line"); + return std::nullopt; } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt) { - RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); - return std::nullopt; - } - idxs.pass_judge_line = insert_idx_opt.value(); - idxs.collision_stop_line = - std::min(idxs.collision_stop_line + 1, original_path->points.size() - 1); + collision_stop_line = insert_idx_opt.value(); } } - RCLCPP_DEBUG( - logger, - "generateStopLine() : stop_idx = %ld, pass_judge_idx = %ld" - ", has_prior_stopline = %d", - idxs.collision_stop_line, idxs.pass_judge_line, has_prior_stopline); + RCLCPP_DEBUG(logger, "generateCollisionStopLine() : stop_idx = %ld", collision_stop_line.value()); - return std::make_optional(idxs); + return collision_stop_line; } std::optional generateStuckStopLine( @@ -386,10 +413,10 @@ bool getStopLineIndexFromMap( IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const std::set & assoc_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const int lane_id, const lanelet::ConstLanelets & lanelets_on_path, + const std::set & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const double detection_area_length, - const bool tl_arrow_solid_on) + const double occlusion_detection_area_length, const bool tl_arrow_solid_on) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); @@ -420,7 +447,7 @@ IntersectionLanelets getObjectiveLanelets( } // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets{}; + lanelet::ConstLanelets ego_lanelets = lanelets_on_path; for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { ego_lanelets.push_back(previous_lanelet); for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { @@ -482,6 +509,27 @@ IntersectionLanelets getObjectiveLanelets( } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + IntersectionLanelets result; if (!tl_arrow_solid_on) { result.attention = std::move(detection_and_preceding_lanelets); @@ -490,10 +538,12 @@ IntersectionLanelets getObjectiveLanelets( } result.conflicting = std::move(conflicting_ex_ego_lanelets); result.adjacent = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, assoc_ids); + result.occlusion_attention = std::move(occlusion_detection_and_preceding_lanelets); // compoundPolygon3d result.attention_area = getPolygon3dFromLanelets(result.attention); result.conflicting_area = getPolygon3dFromLanelets(result.conflicting); result.adjacent_area = getPolygon3dFromLanelets(result.adjacent); + result.occlusion_attention_area = getPolygon3dFromLanelets(result.occlusion_attention); // find the first conflicting/detection area polygon intersecting the path { @@ -698,5 +748,133 @@ bool isTrafficLightArrowActivated( return false; } +std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const double resolution) +{ + using lanelet::utils::getCenterlineWithOffset; + using lanelet::utils::to2D; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + const auto turn_direction = getTurnDirection(detection_lanelet); + if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + // generate adjacency matrix + // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true + const int n_node = detection_lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set detection_lanelet_ids; + std::unordered_map id2ind, ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & detection_lanelet : detection_lanelets) { + detection_lanelet_ids.insert(detection_lanelet.id()); + const int id = detection_lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = detection_lanelet; + ind++; + } + for (const auto & detection_lanelet : detection_lanelets) { + const auto & followings = routing_graph_ptr->following(detection_lanelet); + const int dst = detection_lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); + detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + int node_iter = ind2id[src]; + while (true) { + const auto & dsts = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one previous lanelet + const auto next = std::find(dsts.begin(), dsts.end(), true); + if (next == dsts.end()) { + branch.push_back(node_iter); + break; + } + branch.push_back(node_iter); + node_iter = ind2id[std::distance(dsts.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::unordered_map> merged_branches; + for (const auto & [src, branch] : branches) { + lanelet::Points3d lefts; + lanelet::Points3d rights; + double area = 0; + for (const auto & lane_id : branch) { + const auto lane = id2lanelet[lane_id]; + for (const auto & left_point : lane.leftBound()) { + lefts.push_back(lanelet::Point3d(left_point)); + } + for (const auto & right_point : lane.rightBound()) { + rights.push_back(lanelet::Point3d(right_point)); + } + area += bg::area(lane.polygon2d().basicPolygon()); + } + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); + lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); + merged_branches[src] = std::make_pair(merged, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [last_lane_id, branch] : merged_branches) { + DetectionLaneDivision detection_division; + detection_division.lane_id = last_lane_id; + const auto detection_lanelet = branch.first; + const double area = branch.second; + const double length = bg::length(detection_lanelet.centerline()); + const double width = area / length; + auto & divisions = detection_division.divisions; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + } + divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); + detection_divisions.push_back(detection_division); + } + return detection_divisions; +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp similarity index 75% rename from planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp rename to planning/behavior_velocity_intersection_module/src/util.hpp index b703147d1fab5..980abf2cd005b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__UTIL_HPP_ -#define SCENE_MODULE__INTERSECTION__UTIL_HPP_ +#ifndef UTIL_HPP_ +#define UTIL_HPP_ + +#include "scene_intersection.hpp" +#include "util_type.hpp" #include -#include -#include #include @@ -57,10 +58,10 @@ std::optional getDuplicatedPointIdx( */ IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const std::set & assoc_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const int lane_id, const lanelet::ConstLanelets & lanelets_on_path, + const std::set & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const double detection_area_length, - const bool tl_arrow_solid_on = false); + const double occlusion_detection_area_length, const bool tl_arrow_solid_on = false); /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, @@ -72,7 +73,7 @@ IntersectionLanelets getObjectiveLanelets( " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane * @return nullopt if path is not intersecting with detection areas */ -std::optional generateStopLine( +std::optional generateCollisionStopLine( const int lane_id, const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, @@ -94,6 +95,32 @@ std::optional generateStuckStopLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, const std::pair lane_interval, const rclcpp::Logger logger); +std::optional> generateOcclusionStopLines( + const int lane_id, const std::vector & detection_areas, + const std::shared_ptr & planner_data, const double collision_stop_line_margin, + const size_t occlusion_projection_index, const double occlusion_extra_margin, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval, const rclcpp::Logger logger); + +std::optional generateStaticPassJudgeLine( + const lanelet::CompoundPolygon3d & first_detection_area, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, + const std::shared_ptr & planner_data); + +std::optional generatePeekingLimitLine( + const lanelet::CompoundPolygon3d & first_detection_area, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, + const std::shared_ptr & planner_data, const double offset); + +std::optional getFirstPointInsidePolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + /** * @brief Calculate first path index that is in the polygon. * @param path target path @@ -160,7 +187,11 @@ bool isTrafficLightArrowActivated( lanelet::ConstLanelet lane, const std::map & tl_infos); +std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + } // namespace util } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__UTIL_HPP_ +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp rename to planning/behavior_velocity_intersection_module/src/util_type.hpp index dc6cbd12847e3..5425c5dab4507 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ -#define SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#ifndef UTIL_TYPE_HPP_ +#define UTIL_TYPE_HPP_ #include #include @@ -29,9 +29,11 @@ struct IntersectionLanelets lanelet::ConstLanelets attention; lanelet::ConstLanelets conflicting; lanelet::ConstLanelets adjacent; + lanelet::ConstLanelets occlusion_attention; // for occlusion detection std::vector attention_area; std::vector conflicting_area; std::vector adjacent_area; + std::vector occlusion_attention_area; // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. @@ -39,12 +41,19 @@ struct IntersectionLanelets std::optional first_detection_area; }; -struct StopLineIdx +enum class StopReason : int { + STUCK, + COLLISION, + OCCLUSION, +}; + +struct DetectionLaneDivision { - size_t pass_judge_line = 0; - size_t collision_stop_line = 0; + int lane_id; + // discrete fine lines from left to right + std::vector divisions; }; } // namespace behavior_velocity_planner::util -#endif // SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt new file mode 100644 index 0000000000000..0340710792fda --- /dev/null +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_no_stopping_area_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene_no_stopping_area.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml new file mode 100644 index 0000000000000..a9c21f1aba189 --- /dev/null +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -0,0 +1,46 @@ + + + + behavior_velocity_no_stopping_area_module + 0.1.0 + The behavior_velocity_no_stopping_area_module package + + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + interpolation + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml new file mode 100644 index 0000000000000..a9b07297cfa30 --- /dev/null +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp rename to planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 205d9314c2505..ad6ef3afc38f1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utilization/debug.hpp" - -#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" -#include "utilization/util.hpp" +#include "scene_no_stopping_area.hpp" +#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -167,7 +166,6 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createVirtualWallMark visualization_msgs::msg::MarkerArray wall_marker; const auto now = clock_->now(); - auto id = module_id_; std::vector stop_poses; for (const auto & p : debug_data_.stop_poses) { @@ -176,8 +174,7 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createVirtualWallMark stop_poses.push_back(p_front); } appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker( - stop_poses, "no_stopping_area", now, id), + virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_poses, "no_stopping_area", now), &wall_marker, now); return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp rename to planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index e0a3b207d72ee..e290a70c00386 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/no_stopping_area/manager.hpp" +#include "manager.hpp" #include #include +#include #include #include #include @@ -81,3 +82,7 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp similarity index 72% rename from planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp rename to planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index 0dc4dd7559a9d..f7a9a5433e900 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ -#define SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ -#include "rclcpp/rclcpp.hpp" -#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" -#include "scene_module/scene_module_interface.hpp" +#include "scene_no_stopping_area.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include +#include +#include +#include + +#include #include #include @@ -41,6 +44,11 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class NoStoppingAreaModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp rename to planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index a9c9e583bbfd3..77fbb18be2b5b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" - -#include "utilization/arc_lane_util.hpp" -#include "utilization/path_utilization.hpp" -#include "utilization/util.hpp" +#include "scene_no_stopping_area.hpp" +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp rename to planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index e387b1b39cf2d..304b6b65adfe2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ -#define SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ +#ifndef SCENE_NO_STOPPING_AREA_HPP_ +#define SCENE_NO_STOPPING_AREA_HPP_ #define EIGEN_MPL2_ONLY -#include "scene_module/scene_module_interface.hpp" -#include "utilization/boost_geometry_helper.hpp" -#include "utilization/state_machine.hpp" - #include +#include +#include +#include #include #include @@ -180,4 +179,4 @@ class NoStoppingAreaModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ +#endif // SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt new file mode 100644 index 0000000000000..06e8e8cf3e907 --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_occlusion_spot_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/grid_utils.cpp + src/manager.cpp + src/occlusion_spot_utils.cpp + src/risk_predictive_braking.cpp + src/scene_occlusion_spot.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_occlusion_spot_utils.cpp + test/src/test_risk_predictive_braking.cpp + test/src/test_grid_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package() diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml new file mode 100644 index 0000000000000..cb5bd744edfa8 --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -0,0 +1,47 @@ + + + + behavior_velocity_occlusion_spot_module + 0.1.0 + The behavior_velocity_occlusion_spot_module package + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Taiki Tanaka + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + grid_map_ros + grid_map_utils + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + motion_utils + nav_msgs + pluginlib + rclcpp + route_handler + tf2 + tf2_geometry_msgs + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml new file mode 100644 index 0000000000000..a38900d1a893b --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index bf82bd74552df..737d3bdee1d3e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "occlusion_spot_utils.hpp" +#include "scene_occlusion_spot.hpp" + +#include +#include #include -#include -#include #include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index a9be8abce007f..69ad30e32dfdc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "grid_utils.hpp" #include +#include #include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 538d04cdadde6..849e16eb0834c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ +#ifndef GRID_UTILS_HPP_ +#define GRID_UTILS_HPP_ +#include +#include #include #include #include @@ -22,8 +24,6 @@ #include #include #include -#include -#include #include #include @@ -120,4 +120,4 @@ void denoiseOccupancyGridCV( } // namespace grid_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ +#endif // GRID_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index b6a21dd0acee5..5b1b3d489041f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "manager.hpp" + +#include "scene_occlusion_spot.hpp" + +#include #include @@ -129,3 +131,7 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp similarity index 80% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index 7efb3b522560d..c153a727b8a7e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "occlusion_spot_utils.hpp" +#include "scene_occlusion_spot.hpp" + +#include +#include +#include #include -#include -#include -#include #include #include @@ -55,6 +58,11 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class OcclusionSpotModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index d8507cb5a77c8..0a172c7f5544b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "occlusion_spot_utils.hpp" + +#include "risk_predictive_braking.hpp" + +#include +#include #include #include -#include -#include #include #include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 47ea7b5532278..cc07461864c16 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ +#ifndef OCCLUSION_SPOT_UTILS_HPP_ +#define OCCLUSION_SPOT_UTILS_HPP_ +#include "grid_utils.hpp" + +#include #include #include #include #include -#include -#include #include #include @@ -252,4 +253,4 @@ bool generatePossibleCollisionsFromGridMap( } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ +#endif // OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index f4817428af445..ef181ee7613a0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "risk_predictive_braking.hpp" + +#include "occlusion_spot_utils.hpp" + +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp similarity index 82% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 15eac76e35c48..9a0278bfad5e5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ +#ifndef RISK_PREDICTIVE_BRAKING_HPP_ +#define RISK_PREDICTIVE_BRAKING_HPP_ -#include +#include "occlusion_spot_utils.hpp" #include @@ -40,4 +40,4 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ +#endif // RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index a37154c20f1d7..b7278aff29902 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -12,20 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_occlusion_spot.hpp" + +#include "occlusion_spot_utils.hpp" +#include "risk_predictive_braking.hpp" + +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp similarity index 86% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index c7ee714b03a7b..5632a713eec3c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#ifndef SCENE_OCCLUSION_SPOT_HPP_ +#define SCENE_OCCLUSION_SPOT_HPP_ +#include "occlusion_spot_utils.hpp" + +#include +#include #include -#include -#include #include -#include #include #include @@ -74,4 +75,4 @@ class OcclusionSpotModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#endif // SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp similarity index 98% rename from planning/behavior_velocity_planner/test/src/test_grid_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index 19e9b5f37b028..bc526959386e9 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "grid_utils.hpp" #include "utils.hpp" -#include +#include #include -#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp similarity index 96% rename from planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 93f435590f3e0..ef2e776112766 100644 --- a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -13,12 +13,13 @@ // limitations under the License. #include "gtest/gtest.h" -#include "scene_module/occlusion_spot/occlusion_spot_utils.hpp" +#include "occlusion_spot_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "utilization/path_utilization.hpp" -#include "utilization/util.hpp" #include "utils.hpp" +#include +#include + #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" diff --git a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp similarity index 93% rename from planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp rename to planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp index 6f3a3e4100123..9b1d92b71a111 100644 --- a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "occlusion_spot_utils.hpp" +#include "risk_predictive_braking.hpp" #include "utils.hpp" -#include -#include - #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp new file mode 100644 index 0000000000000..cf24be4cc0b5c --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include "occlusion_spot_utils.hpp" + +#include + +#include +#include + +#include + +namespace test +{ + +inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +// /!\ columns and rows in the GridMap are "inverted" (x -> rows, y -> columns) +inline grid_map::GridMap generateGrid(int w, int h, double res) +{ + grid_map::GridMap grid{}; + grid_map::Length length(w * res, h * res); + grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); + grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + return grid; +} + +using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +inline void generatePossibleCollisions( + std::vector & possible_collisions, double x0, double y0, double x, + double y, int nb_cols) +{ + using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + const double lon = 0.0; // assume col_x = intersection_x + const double lat = -1.0; + const double velocity = 1.0; + /** + * @brief representation of a possible collision between ego and some obstacle + * ^ + * | + * Ego ---------collision----------intersection-------> path + * | + * ------------------ | + * | Vehicle | obstacle + * ------------------ + */ + double x_step = (x - x0) / (nb_cols - 1); + double y_step = (y - y0) / (nb_cols - 1); + for (int i = 0; i < nb_cols; ++i) { + // collision + ObstacleInfo obstacle_info; + obstacle_info.position.x = x0 + x_step * i; + obstacle_info.position.y = y0 + y_step * i + lat; + obstacle_info.max_velocity = velocity; + + // intersection + geometry_msgs::msg::Pose intersection_pose{}; + intersection_pose.position.x = x0 + x_step * i + lon; + intersection_pose.position.x = y0 + y_step * i; + + // collision path point + autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; + collision_with_margin.pose.position.x = x0 + x_step * i + lon; + collision_with_margin.pose.position.y = y0 + y_step * i; + + lanelet::ArcCoordinates arc; + arc.length = obstacle_info.position.x; + arc.distance = obstacle_info.position.y; + + PossibleCollisionInfo col(obstacle_info, collision_with_margin, intersection_pose, arc); + possible_collisions.emplace_back(col); + } +} + +} // namespace test + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..40173ad76d933 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_out_of_lane_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/decisions.cpp + src/footprint.cpp + src/lanelets_selection.cpp + src/manager.cpp + src/overlapping_range.cpp + src/scene_out_of_lane.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml new file mode 100644 index 0000000000000..c2b5c0310c9a7 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_velocity_out_of_lane_module + 0.1.0 + The behavior_velocity_out_of_lane_module package + + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml new file mode 100644 index 0000000000000..8c18fdce79480 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/debug.cpp rename to planning/behavior_velocity_out_of_lane_module/src/debug.cpp index eb69a0655a733..de0b78424f586 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/debug.hpp" +#include "debug.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/debug.hpp rename to planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 7385967bbb09a..06bc6d935f310 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__DEBUG_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__DEBUG_HPP_ +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -52,4 +52,4 @@ void add_lanelet_markers( const std_msgs::msg::ColorRGBA & color); } // namespace behavior_velocity_planner::out_of_lane::debug -#endif // SCENE_MODULE__OUT_OF_LANE__DEBUG_HPP_ +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/decisions.cpp rename to planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 68e96ec3d25e4..a164a03a9aa43 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/decisions.hpp" +#include "decisions.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/decisions.hpp rename to planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 345835ce53250..6af03bc53ea3c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__DECISIONS_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__DECISIONS_HPP_ +#ifndef DECISIONS_HPP_ +#define DECISIONS_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include #include @@ -123,4 +123,4 @@ std::vector calculate_decisions( const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__DECISIONS_HPP_ +#endif // DECISIONS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/footprint.cpp rename to planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 784b8d81e7847..929fbd944af9f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/footprint.hpp" +#include "footprint.hpp" #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/footprint.hpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/footprint.hpp rename to planning/behavior_velocity_out_of_lane_module/src/footprint.hpp index feaa6663e3a23..a573b6ff3981c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/footprint.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__FOOTPRINT_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__FOOTPRINT_HPP_ +#ifndef FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -56,4 +56,4 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( } // namespace out_of_lane } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OUT_OF_LANE__FOOTPRINT_HPP_ +#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/lanelets_selection.cpp rename to planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 35ae9cf3bf00a..a0e2b86caa623 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/lanelets_selection.hpp" +#include "lanelets_selection.hpp" -#include "utilization/util.hpp" +#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/lanelets_selection.hpp rename to planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index 90a92f6580abc..cd0b23768872d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__LANELETS_SELECTION_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__LANELETS_SELECTION_HPP_ +#ifndef LANELETS_SELECTION_HPP_ +#define LANELETS_SELECTION_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -55,4 +55,4 @@ lanelet::ConstLanelets calculate_other_lanelets( const route_handler::RouteHandler & route_handler, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__LANELETS_SELECTION_HPP_ +#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp similarity index 92% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/manager.cpp rename to planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 4c055e9bccfab..1eccd8773cc92 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "manager.hpp" + +#include "scene_out_of_lane.hpp" + +#include #include @@ -85,3 +87,7 @@ OutOfLaneModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp similarity index 81% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/manager.hpp rename to planning/behavior_velocity_out_of_lane_module/src/manager.hpp index 39e18655383c2..c8ef397913c8f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__MANAGER_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene_out_of_lane.hpp" + +#include +#include +#include #include -#include -#include #include #include @@ -53,6 +56,11 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class OutOfLaneModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OUT_OF_LANE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/overlapping_range.cpp rename to planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index fd872f10b8f97..98a9e4404e013 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/overlapping_range.hpp" +#include "overlapping_range.hpp" #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp similarity index 93% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/overlapping_range.hpp rename to planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index 9b417b8efd3ee..ab4d31480e1cb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__OVERLAPPING_RANGE_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__OVERLAPPING_RANGE_HPP_ +#ifndef OVERLAPPING_RANGE_HPP_ +#define OVERLAPPING_RANGE_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -66,4 +66,4 @@ OverlapRanges calculate_overlapping_ranges( const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__OVERLAPPING_RANGE_HPP_ +#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/scene_out_of_lane.cpp rename to planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 6921f90ab3ffb..20260f4dc41f6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -12,25 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/scene_out_of_lane.hpp" +#include "scene_out_of_lane.hpp" -#include "scene_module/out_of_lane/debug.hpp" -#include "scene_module/out_of_lane/decisions.hpp" -#include "scene_module/out_of_lane/footprint.hpp" -#include "scene_module/out_of_lane/lanelets_selection.hpp" -#include "scene_module/out_of_lane/overlapping_range.hpp" -#include "scene_module/out_of_lane/types.hpp" +#include "debug.hpp" +#include "decisions.hpp" +#include "footprint.hpp" +#include "lanelets_selection.hpp" +#include "overlapping_range.hpp" +#include "types.hpp" +#include +#include #include #include #include -#include -#include #include #include #include +#include #include namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp similarity index 89% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/scene_out_of_lane.hpp rename to planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 01ab0856f5525..2d6a3b941cc33 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__SCENE_OUT_OF_LANE_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__SCENE_OUT_OF_LANE_HPP_ +#ifndef SCENE_OUT_OF_LANE_HPP_ +#define SCENE_OUT_OF_LANE_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" +#include #include -#include #include #include @@ -70,4 +70,4 @@ std::vector calculate_slowdown_points( } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__SCENE_OUT_OF_LANE_HPP_ +#endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/types.hpp rename to planning/behavior_velocity_out_of_lane_module/src/types.hpp index 3bee7b1c5eded..e4ba8fbfead81 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__TYPES_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__TYPES_HPP_ +#ifndef TYPES_HPP_ +#define TYPES_HPP_ #include @@ -173,4 +173,4 @@ struct DebugData } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__TYPES_HPP_ +#endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 870791444adf3..fbdb17f93db65 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -4,89 +4,25 @@ project(behavior_velocity_planner) find_package(autoware_cmake REQUIRED) autoware_package() -# Find the non-obvious packages manually -find_package(Boost REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS common filters) -find_package(OpenCV REQUIRED) - -set(scene_modules - detection_area - blind_spot - stop_line - crosswalk - traffic_light - intersection - no_stopping_area - virtual_traffic_light - occlusion_spot - run_out - speed_bump - out_of_lane -) - -foreach(scene_module IN LISTS scene_modules) - file(GLOB_RECURSE scene_module_src "src/scene_module/${scene_module}/*") - list(APPEND scene_modules_src ${scene_module_src}) -endforeach() - -ament_auto_add_library(behavior_velocity_planner SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp src/planner_manager.cpp - src/utilization/path_utilization.cpp - src/utilization/util.cpp - src/utilization/debug.cpp - ${scene_modules_src} ) -target_include_directories(behavior_velocity_planner - SYSTEM PUBLIC - ${BOOST_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${tf2_geometry_msgs_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) - -ament_target_dependencies(behavior_velocity_planner - Boost - Eigen3 - PCL - message_filters -) - -target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES}) - -rclcpp_components_register_node(behavior_velocity_planner +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" - EXECUTABLE behavior_velocity_planner_node + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) - # Gtest for utilization - ament_add_ros_isolated_gtest(utilization-test - test/src/test_state_machine.cpp - test/src/test_arc_lane_util.cpp - test/src/test_utilization.cpp - ) - target_link_libraries(utilization-test - gtest_main - behavior_velocity_planner - ) - - # Gtest for occlusion spot - ament_add_ros_isolated_gtest(occlusion_spot-test - test/src/test_occlusion_spot_utils.cpp - test/src/test_risk_predictive_braking.cpp - test/src/test_grid_utils.cpp + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_node_interface.cpp ) - # Gtest for out of lane - # ament_add_ros_isolated_gtest(out_of_lane-test - # ) - target_link_libraries(occlusion_spot-test + target_link_libraries(test_${PROJECT_NAME} gtest_main - behavior_velocity_planner + ${PROJECT_NAME} ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 8a69d80394a68..d10d1093fc789 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -11,6 +11,7 @@ intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection + use_intersection_area: false stuck_vehicle: use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck @@ -28,5 +29,19 @@ collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + occlusion: + enable: false + occlusion_detection_area_length: 70.0 # [m] + enable_creeping: false # flag to use the creep velocity when reaching occlusion limit stop line + occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line + peeking_offset: -0.1 # [m] offset for peeking into detection area + free_space_max: 43 + occupied_min: 58 + do_dp: true + before_creep_stop_time: 1.0 # [s] + min_vehicle_brake_for_rss: -2.5 # [m/s^2] + max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph + denoise_kernel: 1.0 # [m] + merge_from_private: stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/out-of-lane-design.md b/planning/behavior_velocity_planner/out-of-lane-design.md index 942a2516c0612..2e732d04027d6 100644 --- a/planning/behavior_velocity_planner/out-of-lane-design.md +++ b/planning/behavior_velocity_planner/out-of-lane-design.md @@ -27,10 +27,10 @@ In this first step, the ego footprint is projected at each path point and are ev #### 2. Other lanes In the second step, the set of lanes to consider for overlaps is generated. -This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevent lanelets. -The selection distance is choosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. +This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. +The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. -A lanelet is deemed non-relevent if it meets one of the following conditions. +A lanelet is deemed non-relevant if it meets one of the following conditions. - It is part of the lanelets followed by the ego path. - It contains the rear point of the ego footprint. diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index f2de760a3e0f8..0f131eebcd40c 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -5,32 +5,16 @@ 0.1.0 The behavior_velocity_planner package - Mamoru Sobue Takayuki Murooka - Satoshi Ota - Kyoichi Sugahara - Taiki Tanaka - Kosuke Takeuchi - - Satoshi Ota - - Mamoru Sobue - Yutaka Shimizu - - Kosuke Takeuchi - Tomohito Ando Makoto Kurihara - Maxime Clement - - Tomoya Kimura Shumpei Wakabayashi @@ -45,35 +29,26 @@ Tomohito Ando Yukihiro Saito - ament_cmake + ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake - - autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 + behavior_velocity_planner_common diagnostic_msgs eigen geometry_msgs - grid_map_ros - grid_map_utils - interpolation lanelet2_extension libboost-dev - libopencv-dev - message_filters motion_utils motion_velocity_smoother - nav_msgs - nlohmann-json-dev pcl_conversions + pluginlib rclcpp rclcpp_components route_handler - rtc_interface sensor_msgs tf2 tf2_eigen @@ -83,7 +58,6 @@ tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 2d3277c810606..3645ffca33454 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_velocity_planner/node.hpp" +#include "node.hpp" +#include #include #include -#include #include #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -33,20 +34,6 @@ #include #include -// Scene modules -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -150,46 +137,53 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager if (this->declare_parameter("launch_crosswalk")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::CrosswalkModulePlugin"); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::WalkwayModulePlugin"); } if (this->declare_parameter("launch_traffic_light")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::TrafficLightModulePlugin"); } if (this->declare_parameter("launch_intersection")) { // intersection module should be before merge from private to declare intersection parameters - planner_manager_.launchSceneModule(std::make_shared(*this)); - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::IntersectionModulePlugin"); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::MergeFromPrivateModulePlugin"); } if (this->declare_parameter("launch_blind_spot")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::BlindSpotModulePlugin"); } if (this->declare_parameter("launch_detection_area")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::DetectionAreaModulePlugin"); } if (this->declare_parameter("launch_virtual_traffic_light")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::VirtualTrafficLightModulePlugin"); } // this module requires all the stop line.Therefore this modules should be placed at the bottom. if (this->declare_parameter("launch_no_stopping_area")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::NoStoppingAreaModulePlugin"); } // permanent stop line module should be after no stopping area if (this->declare_parameter("launch_stop_line")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::StopLineModulePlugin"); } // to calculate ttc it's better to be after stop line if (this->declare_parameter("launch_occlusion_spot")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::OcclusionSpotModulePlugin"); } if (this->declare_parameter("launch_run_out")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::RunOutModulePlugin"); } if (this->declare_parameter("launch_speed_bump")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::SpeedBumpModulePlugin"); } if (this->declare_parameter("launch_out_of_lane")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::OutOfLaneModulePlugin"); } } @@ -230,6 +224,11 @@ bool BehaviorVelocityPlannerNode::isDataReady( get_logger(), clock, 3000, "Waiting for the initialization of velocity smoother"); return false; } + if (!d.occupancy_grid) { + RCLCPP_INFO_THROTTLE( + get_logger(), clock, 3000, "Waiting for the initialization of occupancy grid map"); + return false; + } return true; } @@ -265,7 +264,7 @@ void BehaviorVelocityPlannerNode::onNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); if (!pc.empty()) { - pcl::transformPointCloud(pc, *pc_transformed, affine); + tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); } { @@ -316,9 +315,9 @@ void BehaviorVelocityPlannerNode::onAcceleration( void BehaviorVelocityPlannerNode::onParam() { - // Note(vrichard): mutex lock is not necessary as onParam is only called once in the constructed. - // It would be required if it was a callback. - // std::lock_guard lock(mutex_); + // Note(VRichardJP): mutex lock is not necessary as onParam is only called once in the + // constructed. It would be required if it was a callback. std::lock_guard + // lock(mutex_); planner_data_.velocity_smoother_ = std::make_unique(*this); } diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/src/node.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp rename to planning/behavior_velocity_planner/src/node.hpp index 6e8f3a33c8d67..91172fdd77da8 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#ifndef NODE_HPP_ +#define NODE_HPP_ -#include "behavior_velocity_planner/planner_data.hpp" -#include "behavior_velocity_planner/planner_manager.hpp" +#include "planner_manager.hpp" +#include #include #include @@ -114,4 +114,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node }; } // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 374ddaa234a77..39d4dc345e899 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_velocity_planner/planner_manager.hpp" +#include "planner_manager.hpp" #include @@ -49,10 +49,21 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } } // namespace -void BehaviorVelocityPlannerManager::launchSceneModule( - const std::shared_ptr & scene_module_manager_ptr) +BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() +: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { - scene_manager_ptrs_.push_back(scene_module_manager_ptr); +} + +void BehaviorVelocityPlannerManager::launchScenePlugin( + rclcpp::Node & node, const std::string & name) +{ + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node); + scene_manager_plugins_.push_back(plugin); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } } autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( @@ -64,15 +75,15 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: int first_stop_path_point_index = static_cast(output_path_msg.points.size() - 1); std::string stop_reason_msg("path_end"); - for (const auto & scene_manager_ptr : scene_manager_ptrs_) { - scene_manager_ptr->updateSceneModuleInstances(planner_data, input_path_msg); - scene_manager_ptr->plan(&output_path_msg); - boost::optional firstStopPathPointIndex = scene_manager_ptr->getFirstStopPathPointIndex(); + for (const auto & plugin : scene_manager_plugins_) { + plugin->updateSceneModuleInstances(planner_data, input_path_msg); + plugin->plan(&output_path_msg); + boost::optional firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); if (firstStopPathPointIndex) { if (firstStopPathPointIndex.get() < first_stop_path_point_index) { first_stop_path_point_index = firstStopPathPointIndex.get(); - stop_reason_msg = scene_manager_ptr->getModuleName(); + stop_reason_msg = plugin->getModuleName(); } } } diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp similarity index 77% rename from planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp rename to planning/behavior_velocity_planner/src/planner_manager.hpp index 3b24c72474e87..e57d90d5cdfee 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#ifndef PLANNER_MANAGER_HPP_ +#define PLANNER_MANAGER_HPP_ +#include +#include +#include #include -#include #include #include @@ -39,8 +41,8 @@ namespace behavior_velocity_planner class BehaviorVelocityPlannerManager { public: - void launchSceneModule( - const std::shared_ptr & scene_module_manager_ptr); + BehaviorVelocityPlannerManager(); + void launchScenePlugin(rclcpp::Node & node, const std::string & name); autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, @@ -49,9 +51,10 @@ class BehaviorVelocityPlannerManager diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; private: - std::vector> scene_manager_ptrs_; diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; + pluginlib::ClassLoader plugin_loader_; + std::vector> scene_manager_plugins_; }; } // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#endif // PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp new file mode 100644 index 0000000000000..061a46f01f219 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -0,0 +1,156 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "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 + +using behavior_velocity_planner::BehaviorVelocityPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_velocity_planner → test_node_ + test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); + + // set behavior_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setPathWithLaneIdTopicName( + "behavior_velocity_planner_node/input/path_with_lane_id"); + + test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); + const auto motion_velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", + motion_velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", + behavior_velocity_planner_dir + "/config/blind_spot.param.yaml", + behavior_velocity_planner_dir + "/config/crosswalk.param.yaml", + behavior_velocity_planner_dir + "/config/detection_area.param.yaml", + behavior_velocity_planner_dir + "/config/intersection.param.yaml", + behavior_velocity_planner_dir + "/config/no_stopping_area.param.yaml", + behavior_velocity_planner_dir + "/config/occlusion_spot.param.yaml", + behavior_velocity_planner_dir + "/config/run_out.param.yaml", + behavior_velocity_planner_dir + "/config/speed_bump.param.yaml", + behavior_velocity_planner_dir + "/config/stop_line.param.yaml", + behavior_velocity_planner_dir + "/config/traffic_light.param.yaml", + behavior_velocity_planner_dir + "/config/virtual_traffic_light.param.yaml", + behavior_velocity_planner_dir + "/config/out_of_lane.param.yaml"}); + + node_options.append_parameter_override("launch_stop_line", true); + node_options.append_parameter_override("launch_crosswalk", true); + node_options.append_parameter_override("launch_traffic_light", true); + node_options.append_parameter_override("launch_intersection", true); + node_options.append_parameter_override("launch_blind_spot", true); + node_options.append_parameter_override("launch_detection_area", true); + node_options.append_parameter_override( + "launch_virtual_traffic_light", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override( + "launch_occlusion_spot", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override("launch_no_stopping_area", true); + node_options.append_parameter_override( + "launch_run_out", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override( + "launch_speed_bump", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override("launch_out_of_lane", true); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); + test_manager->publishMaxVelocity( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_velocity_planner/test/src/utils.hpp b/planning/behavior_velocity_planner/test/src/utils.hpp deleted file mode 100644 index 91574b02c65e2..0000000000000 --- a/planning/behavior_velocity_planner/test/src/utils.hpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UTILS_HPP_ -#define UTILS_HPP_ - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace test -{ -// Default grid parameter such that UNKNOWN cells have a value of 50 -const behavior_velocity_planner::grid_utils::GridParam grid_param = {10, 51}; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using ConstPair = const std::pair; - -/* lanelet - 0 1 2 3 4 5 - 0 x - 1 x - 2 x - 3 x - 4 x - 5 x - */ -inline lanelet::ConstLanelet createLanelet( - ConstPair & start = {0, 0}, ConstPair & end = {5, 5}, int nb_points = 6) -{ - const double interval = - std::hypot(end.first - start.first, end.second - start.second) / (nb_points - 1); - const double norm_x = (end.first - start.first) / interval; - const double norm_y = (end.second - start.second) / interval; - lanelet::Points3d path_points; - for (int i = 0; i < nb_points; i++) { - const double x = start.first + norm_x * i; - const double y = start.second + norm_y * i; - path_points.emplace_back(lanelet::InvalId, x, y, 0); - } - lanelet::LineString3d centerline(lanelet::InvalId, path_points); - lanelet::Lanelet path_lanelet(lanelet::InvalId); - path_lanelet.setCenterline(centerline); - return lanelet::ConstLanelet(path_lanelet); -} - -/* Horizontal lanelet - 0 1 2 3 4 5 6 - 0 x x x x x x x - 1 - 2 x x x x x x x - 3 - 4 - 5 - */ -inline lanelet::ConstLanelet horizontalLanelet( - std::pair origin = {0, 0}, double width = 2.0, double length = 6.0, - int nb_points = 2) -{ - lanelet::Lanelet l; - lanelet::Points3d line; - for (double x = origin.first; x <= origin.first + length; x += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, x, origin.second)); - } - l.setLeftBound(lanelet::LineString3d(0, line)); - line.clear(); - for (double x = origin.second; x <= origin.first + length; x += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, x, origin.second + width)); - } - l.setRightBound(lanelet::LineString3d(1, line)); - return lanelet::ConstLanelet(l); -} -/* Vertical lanelet - 0 1 2 3 4 5 6 - 0 x x - 1 x x - 2 x x - 3 x x - 4 x x - 5 x x - */ -inline lanelet::ConstLanelet verticalLanelet( - std::pair origin = {0, 0}, double width = 2.0, double length = 5.0, - int nb_points = 2) -{ - lanelet::Lanelet l; - lanelet::Points3d line; - for (double y = origin.second; y <= origin.second + length; y += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, origin.first, y)); - } - l.setLeftBound(lanelet::LineString3d(0, line)); - line.clear(); - for (double y = origin.second; y <= origin.second + length; y += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, origin.first + width, y)); - } - l.setRightBound(lanelet::LineString3d(1, line)); - return lanelet::ConstLanelet(l); -} - -// /!\ columns and rows in the GridMap are "inverted" (x -> rows, y -> columns) -inline grid_map::GridMap generateGrid(int w, int h, double res) -{ - grid_map::GridMap grid{}; - grid_map::Length length(w * res, h * res); - grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); - grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); - return grid; -} - -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( - double x0, double y0, double x, double y, int nb_points) -{ - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; - double x_step = (x - x0) / (nb_points - 1); - double y_step = (y - y0) / (nb_points - 1); - for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; - point.point.pose.position.x = x0 + x_step * i; - point.point.pose.position.y = y0 + y_step * i; - point.point.pose.position.z = 0.0; - path.points.push_back(point); - } - return path; -} - -using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; -inline void generatePossibleCollisions( - std::vector & possible_collisions, double x0, double y0, double x, - double y, int nb_cols) -{ - using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; - const double lon = 0.0; // assume col_x = intersection_x - const double lat = -1.0; - const double velocity = 1.0; - /** - * @brief representation of a possible collision between ego and some obstacle - * ^ - * | - * Ego ---------collision----------intersection-------> path - * | - * ------------------ | - * | Vehicle | obstacle - * ------------------ - */ - double x_step = (x - x0) / (nb_cols - 1); - double y_step = (y - y0) / (nb_cols - 1); - for (int i = 0; i < nb_cols; ++i) { - // collision - ObstacleInfo obstacle_info; - obstacle_info.position.x = x0 + x_step * i; - obstacle_info.position.y = y0 + y_step * i + lat; - obstacle_info.max_velocity = velocity; - - // intersection - geometry_msgs::msg::Pose intersection_pose{}; - intersection_pose.position.x = x0 + x_step * i + lon; - intersection_pose.position.x = y0 + y_step * i; - - // collision path point - autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; - collision_with_margin.pose.position.x = x0 + x_step * i + lon; - collision_with_margin.pose.position.y = y0 + y_step * i; - - lanelet::ArcCoordinates arc; - arc.length = obstacle_info.position.x; - arc.distance = obstacle_info.position.y; - - PossibleCollisionInfo col(obstacle_info, collision_with_margin, intersection_pose, arc); - possible_collisions.emplace_back(col); - } -} -inline void addConstantVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId & trajectory, double velocity) -{ - for (auto & p : trajectory.points) { - p.point.longitudinal_velocity_mps = velocity; - } -} -inline autoware_auto_perception_msgs::msg::PredictedObject generatePredictedObject(double x) -{ - autoware_auto_perception_msgs::msg::PredictedObject obj; - obj.shape.dimensions.x = 5.0; - obj.shape.dimensions.y = 2.0; - obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; - obj.kinematics.initial_pose_with_covariance.pose.position.x = x; - obj.kinematics.initial_pose_with_covariance.pose.position.y = 0; - obj.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); - obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - return obj; -} -inline geometry_msgs::msg::Pose generatePose(double x) -{ - geometry_msgs::msg::Pose p; - p.position.x = x; - p.position.y = 0.0; - p.position.z = 1.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0); - p.orientation = tf2::toMsg(q); - return p; -} - -} // namespace test - -#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..334ee35329ba3 --- /dev/null +++ b/planning/behavior_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/utilization/path_utilization.cpp + src/utilization/util.cpp + src/utilization/debug.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_state_machine.cpp + test/src/test_arc_lane_util.cpp + test/src/test_utilization.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index ab702249c707c..44d3184a5f6b7 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #include "route_handler/route_handler.hpp" @@ -144,4 +144,4 @@ struct PlannerData }; } // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp new file mode 100644 index 0000000000000..e5606d7d10dff --- /dev/null +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ + +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ + +class PluginInterface +{ +public: + virtual ~PluginInterface() = default; + virtual void init(rclcpp::Node & node) = 0; + virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual boost::optional getFirstStopPathPointIndex() = 0; + virtual const char * getModuleName() = 0; +}; + +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp new file mode 100644 index 0000000000000..623649f82bf24 --- /dev/null +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ + +#include + +#include + +namespace behavior_velocity_planner +{ + +template +class PluginWrapper : public PluginInterface +{ +public: + void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } + void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override + { + scene_manager_->plan(path); + }; + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override + { + scene_manager_->updateSceneModuleInstances(planner_data, path); + } + boost::optional getFirstStopPathPointIndex() override + { + return scene_manager_->getFirstStopPathPointIndex(); + } + const char * getModuleName() override { return scene_manager_->getModuleName(); } + +private: + std::unique_ptr scene_manager_; +}; + +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index bd786a00b655d..0b3dfd1213eb1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ -#define SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ - -#include "behavior_velocity_planner/planner_data.hpp" -#include "velocity_factor_interface.hpp" +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#include +#include +#include #include #include #include -#include #include #include @@ -58,6 +57,7 @@ using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; +using tier4_rtc_msgs::msg::Module; using unique_identifier_msgs::msg::UUID; class SceneModuleInterface @@ -66,6 +66,7 @@ class SceneModuleInterface explicit SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) : module_id_(module_id), + activated_(false), safe_(false), distance_(std::numeric_limits::lowest()), logger_(logger), @@ -126,8 +127,7 @@ class SceneModuleInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, - p->ego_nearest_yaw_threshold); + points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } }; @@ -135,7 +135,7 @@ class SceneModuleManagerInterface { public: SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) - : clock_(node.get_clock()), logger_(node.get_logger()) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) { const auto ns = std::string("~/debug/") + module_name; pub_debug_ = node.create_publisher(ns, 1); @@ -307,6 +307,7 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; boost::optional first_stop_path_point_index_; + rclcpp::Node & node_; rclcpp::Clock::SharedPtr clock_; // Debug bool is_publish_debug_path_ = {false}; // note : this is very heavy debug topic option @@ -342,7 +343,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface RTCInterface rtc_interface_; std::unordered_map map_uuid_; - void sendRTC(const Time & stamp) + virtual void sendRTC(const Time & stamp) { for (const auto & scene_module : scene_modules_) { const UUID uuid = getUUID(scene_module->getModuleId()); @@ -351,7 +352,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface publishRTCStatus(stamp); } - void setActivation() + virtual void setActivation() { for (const auto & scene_module : scene_modules_) { const UUID uuid = getUUID(scene_module->getModuleId()); @@ -412,4 +413,4 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index dc282785f330d..ffc216e863500 100644 --- a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__ARC_LANE_UTIL_HPP_ -#define UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#include #include #include -#include #include @@ -293,4 +293,4 @@ inline boost::optional createTargetPoint( } // namespace arc_lane_utils } // namespace behavior_velocity_planner -#endif // UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 896f350abe9aa..193890ec70ceb 100644 --- a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #include @@ -127,4 +127,4 @@ inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) } } // namespace behavior_velocity_planner -#endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp similarity index 87% rename from planning/behavior_velocity_planner/include/utilization/debug.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index af398ae539a1e..e840b6f8a5995 100644 --- a/planning/behavior_velocity_planner/include/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__DEBUG_HPP_ -#define UTILIZATION__DEBUG_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#include +#include #include @@ -44,4 +44,4 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const double r, const double g, const double b); } // namespace debug } // namespace behavior_velocity_planner -#endif // UTILIZATION__DEBUG_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 86% rename from planning/behavior_velocity_planner/include/utilization/path_utilization.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 7856a42114619..4156f7df9540b 100644 --- a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__PATH_UTILIZATION_HPP_ -#define UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include #include @@ -37,4 +37,4 @@ autoware_auto_planning_msgs::msg::Path filterStopPathPoint( const autoware_auto_planning_msgs::msg::Path & path); } // namespace behavior_velocity_planner -#endif // UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/utilization/state_machine.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp index b0b78d458e78f..73b0fa7d553a3 100644 --- a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__STATE_MACHINE_HPP_ -#define UTILIZATION__STATE_MACHINE_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ #include @@ -93,4 +93,4 @@ class StateMachine }; } // namespace behavior_velocity_planner -#endif // UTILIZATION__STATE_MACHINE_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index ce04fe9d5c7a9..7d5278c3cf972 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__TRAJECTORY_UTILS_HPP_ -#define UTILIZATION__TRAJECTORY_UTILS_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#include +#include +#include #include #include #include #include -#include #include #include @@ -129,4 +129,4 @@ inline bool smoothPath( } // namespace behavior_velocity_planner -#endif // UTILIZATION__TRAJECTORY_UTILS_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/utilization/util.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index f8cc7910ad649..14b009ceb0748 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__UTIL_HPP_ -#define UTILIZATION__UTIL_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#include #include #include #include -#include #include #include @@ -317,4 +317,4 @@ lanelet::ConstLanelets getConstLaneletsFromIds( } // namespace planning_utils } // namespace behavior_velocity_planner -#endif // UTILIZATION__UTIL_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp similarity index 90% rename from planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp index cff6e54b4f134..28ac40f0868a0 100644 --- a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ -#define SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ #include @@ -65,4 +65,4 @@ class VelocityFactorInterface } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml new file mode 100644 index 0000000000000..293eee96dffdf --- /dev/null +++ b/planning/behavior_velocity_planner_common/package.xml @@ -0,0 +1,66 @@ + + + + behavior_velocity_planner_common + 0.1.0 + The behavior_velocity_planner_common package + + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + + Apache License 2.0 + + Yukihiro Saito + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_adapi_v1_msgs + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + cv_bridge + diagnostic_msgs + eigen + geometry_msgs + grid_map_cv + grid_map_ros + grid_map_utils + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + magic_enum + message_filters + motion_utils + motion_velocity_smoother + nav_msgs + nlohmann-json-dev + pcl_conversions + rclcpp + rclcpp_components + route_handler + rtc_interface + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_api_msgs + tier4_autoware_utils + tier4_planning_msgs + tier4_v2x_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/utilization/debug.cpp rename to planning/behavior_velocity_planner_common/src/utilization/debug.cpp index ec3bfef1fa2de..dc1eaeacb2f71 100644 --- a/planning/behavior_velocity_planner/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/utilization/path_utilization.cpp rename to planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index d570492fd93b3..34d4cf0efa7ac 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include #include -#include #include diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/utilization/util.cpp rename to planning/behavior_velocity_planner_common/src/utilization/util.cpp index db5520a9142d5..d98e9002b2057 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -676,17 +676,18 @@ std::set getAssociativeIntersectionLanelets( const auto neighbors = routing_graph->besides(parent); for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id()); } - std::set assocs; + std::set associative_intersection_lanelets; + associative_intersection_lanelets.insert(lane.id()); for (const auto & parent_neighbor_id : parent_neighbors) { const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id); const auto followings = routing_graph->following(parent_neighbor); for (const auto & following : followings) { if (following.attributeOr("turn_direction", "else") == turn_direction) { - assocs.insert(following.id()); + associative_intersection_lanelets.insert(following.id()); } } } - return assocs; + return associative_intersection_lanelets; } } // namespace planning_utils diff --git a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp similarity index 98% rename from planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp rename to planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index ac5a5c82d6730..c4fdb83e00609 100644 --- a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -14,8 +14,8 @@ #include "utils.hpp" -#include -#include +#include +#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp b/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp similarity index 97% rename from planning/behavior_velocity_planner/test/src/test_state_machine.cpp rename to planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp index 34a6b5b0767b4..ac7c6655ced87 100644 --- a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -14,8 +14,8 @@ #include "utils.hpp" +#include #include -#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp similarity index 96% rename from planning/behavior_velocity_planner/test/src/test_utilization.cpp rename to planning/behavior_velocity_planner_common/test/src/test_utilization.cpp index 742cccecb9773..f43450800b46e 100644 --- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -13,11 +13,11 @@ // limitations under the License. #include "motion_utils/trajectory/trajectory.hpp" -#include "utilization/path_utilization.hpp" #include "utils.hpp" -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner_common/test/src/utils.hpp new file mode 100644 index 0000000000000..1328edb6f6027 --- /dev/null +++ b/planning/behavior_velocity_planner_common/test/src/utils.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#include + +namespace test +{ + +inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +inline geometry_msgs::msg::Pose generatePose(double x) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = 0.0; + p.position.z = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + p.orientation = tf2::toMsg(q); + return p; +} + +} // namespace test + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt new file mode 100644 index 0000000000000..806097cdff865 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_run_out_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/dynamic_obstacle.cpp + src/manager.cpp + src/scene.cpp + src/state_machine.cpp + src/utils.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml new file mode 100644 index 0000000000000..fb3c49bb750a6 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -0,0 +1,46 @@ + + + + behavior_velocity_run_out_module + 0.1.0 + The behavior_velocity_run_out_module package + + Tomohito Ando + Makoto Kurihara + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Tomohito Ando + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + libboost-dev + message_filters + motion_utils + pcl_conversions + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tf2_eigen + tf2_ros + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_run_out_module/plugins.xml b/planning/behavior_velocity_run_out_module/plugins.xml new file mode 100644 index 0000000000000..5320d46f92929 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp rename to planning/behavior_velocity_run_out_module/src/debug.cpp index c679ec0d6ae09..f47baae67f714 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/debug.hpp" +#include "debug.hpp" -#include "scene_module/run_out/scene.hpp" +#include "scene.hpp" #include @@ -179,10 +179,8 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVirtualWallMarkerArray() visualization_msgs::msg::MarkerArray wall_marker; rclcpp::Time now = node_.now(); - size_t id = 0; - appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_pose_, "run_out", now, id), + virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_pose_, "run_out", now), &wall_marker, now); stop_pose_.clear(); diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp rename to planning/behavior_velocity_run_out_module/src/debug.hpp index 37f9399d6a525..e281f323f56cb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__DEBUG_HPP_ -#define SCENE_MODULE__RUN_OUT__DEBUG_HPP_ +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ -#include "scene_module/run_out/utils.hpp" +#include "utils.hpp" #include #include @@ -145,4 +145,4 @@ class RunOutDebug } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__DEBUG_HPP_ +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp rename to planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 290cf9af97dd0..e2f9880435d5e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/dynamic_obstacle.hpp" +#include "dynamic_obstacle.hpp" #include +#include +#include + namespace behavior_velocity_planner { namespace @@ -252,7 +255,8 @@ pcl::PointCloud transformPointCloud( pcl::PointCloud pointcloud_pcl; pcl::fromROSMsg(input_pointcloud, pointcloud_pcl); pcl::PointCloud pointcloud_pcl_transformed; - pcl::transformPointCloud(pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix); + tier4_autoware_utils::transformPointCloud( + pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix); return pointcloud_pcl_transformed; } diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp similarity index 93% rename from planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp rename to planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 7a4fa3377e4d2..f9b2b12298d2d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ -#define SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ +#ifndef DYNAMIC_OBSTACLE_HPP_ +#define DYNAMIC_OBSTACLE_HPP_ -#include "behavior_velocity_planner/planner_data.hpp" -#include "scene_module/run_out/debug.hpp" -#include "scene_module/run_out/utils.hpp" -#include "utilization/path_utilization.hpp" -#include "utilization/util.hpp" +#include "debug.hpp" +#include "utils.hpp" +#include +#include +#include #include #include @@ -171,4 +171,4 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ +#endif // DYNAMIC_OBSTACLE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp rename to planning/behavior_velocity_run_out_module/src/manager.cpp index 884140000e74c..8887f0749a2d5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/manager.hpp" +#include "manager.hpp" + +#include +#include namespace behavior_velocity_planner { @@ -182,3 +185,7 @@ void RunOutModuleManager::setDynamicObstacleCreator( } } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::RunOutModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp similarity index 79% rename from planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp rename to planning/behavior_velocity_run_out_module/src/manager.hpp index ea31cf198fa30..4dd66ad45898b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__MANAGER_HPP_ -#define SCENE_MODULE__RUN_OUT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ -#include "scene_module/run_out/scene.hpp" -#include "scene_module/scene_module_interface.hpp" +#include "scene.hpp" + +#include +#include +#include #include @@ -41,6 +44,11 @@ class RunOutModuleManager : public SceneModuleManagerInterface void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; + +class RunOutModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp similarity index 90% rename from planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp rename to planning/behavior_velocity_run_out_module/src/path_utils.hpp index 50a0f8728f0b3..b01ee70f8bdfa 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ -#define SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ +#ifndef PATH_UTILS_HPP_ +#define PATH_UTILS_HPP_ #include @@ -50,4 +50,4 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( } // namespace run_out_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ +#endif // PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp rename to planning/behavior_velocity_run_out_module/src/scene.cpp index ee8c2e5d0ab75..3b2ed6a78a96c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -12,11 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/scene.hpp" +#include "scene.hpp" -#include "scene_module/run_out/path_utils.hpp" -#include "utilization/trajectory_utils.hpp" -#include "utilization/util.hpp" +#include "path_utils.hpp" + +#include +#include + +#include +#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp rename to planning/behavior_velocity_run_out_module/src/scene.hpp index 3a04065f52c75..017ed0cb17c50 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__SCENE_HPP_ -#define SCENE_MODULE__RUN_OUT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ -#include "scene_module/run_out/debug.hpp" -#include "scene_module/run_out/dynamic_obstacle.hpp" -#include "scene_module/run_out/state_machine.hpp" -#include "scene_module/run_out/utils.hpp" -#include "scene_module/scene_module_interface.hpp" +#include "debug.hpp" +#include "dynamic_obstacle.hpp" +#include "state_machine.hpp" +#include "utils.hpp" + +#include #include #include @@ -143,4 +144,4 @@ class RunOutModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp b/planning/behavior_velocity_run_out_module/src/state_machine.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp rename to planning/behavior_velocity_run_out_module/src/state_machine.cpp index daeb86393b1e9..322264cad8501 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp +++ b/planning/behavior_velocity_run_out_module/src/state_machine.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/state_machine.hpp" +#include "state_machine.hpp" namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp b/planning/behavior_velocity_run_out_module/src/state_machine.hpp similarity index 89% rename from planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp rename to planning/behavior_velocity_run_out_module/src/state_machine.hpp index 0ee34bf6c9685..d3339fa0ef129 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp +++ b/planning/behavior_velocity_run_out_module/src/state_machine.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ -#define SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ +#ifndef STATE_MACHINE_HPP_ +#define STATE_MACHINE_HPP_ -#include "scene_module/run_out/utils.hpp" +#include "utils.hpp" #include #include @@ -59,4 +59,4 @@ class StateMachine } // namespace run_out_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ +#endif // STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp rename to planning/behavior_velocity_run_out_module/src/utils.cpp index 2093d0a01f453..3def2ade46731 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/utils.hpp" +#include "utils.hpp" + +#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp rename to planning/behavior_velocity_run_out_module/src/utils.hpp index 39f41e9da11ff..1f725291a38b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__UTILS_HPP_ -#define SCENE_MODULE__RUN_OUT__UTILS_HPP_ - -#include "behavior_velocity_planner/planner_data.hpp" -#include "utilization/util.hpp" +#ifndef UTILS_HPP_ +#define UTILS_HPP_ +#include +#include #include #include @@ -243,4 +242,4 @@ Polygons2d createMandatoryDetectionAreaPolygon( const PlannerParam & planner_param); } // namespace run_out_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__UTILS_HPP_ +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt new file mode 100644 index 0000000000000..45931b98e502f --- /dev/null +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_speed_bump_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp + src/util.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml new file mode 100644 index 0000000000000..2f10b111c043e --- /dev/null +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_velocity_speed_bump_module + 0.1.0 + The behavior_velocity_speed_bump_module package + + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml new file mode 100644 index 0000000000000..506d25669f8cf --- /dev/null +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp rename to planning/behavior_velocity_speed_bump_module/src/debug.cpp index a51a14f747f4b..6e92672eb51f1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/speed_bump/scene.hpp" +#include "scene.hpp" +#include #include #include -#include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp rename to planning/behavior_velocity_speed_bump_module/src/manager.cpp index f6534aa1610aa..e502963e60b47 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/speed_bump/manager.hpp" +#include "manager.hpp" #include @@ -78,3 +78,7 @@ SpeedBumpModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp rename to planning/behavior_velocity_speed_bump_module/src/manager.hpp index d64870f6a95d4..480208067d87e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ -#define SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -41,6 +44,11 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class SpeedBumpModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp rename to planning/behavior_velocity_speed_bump_module/src/scene.cpp index 49506e5bcc8f2..fda92f037cf7a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/speed_bump/scene.hpp" +#include "scene.hpp" #include "motion_utils/motion_utils.hpp" -#include "scene_module/speed_bump/util.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "util.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp rename to planning/behavior_velocity_speed_bump_module/src/scene.hpp index 74f9da149386e..bf8baf92dae00 100644 --- a/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ -#define SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ +#include "util.hpp" + +#include #include -#include -#include #include #include @@ -81,4 +82,4 @@ class SpeedBumpModule : public SceneModuleInterface } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp rename to planning/behavior_velocity_speed_bump_module/src/util.cpp index 7a8bd9edfd515..89845a4b5e271 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "util.hpp" + #include "motion_utils/motion_utils.hpp" -#include +#include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp rename to planning/behavior_velocity_speed_bump_module/src/util.hpp index 2aafee4a113ab..5b23661431001 100644 --- a/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ -#define SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ +#ifndef UTIL_HPP_ +#define UTIL_HPP_ #include #include @@ -30,6 +30,7 @@ #include #include +#include #include @@ -70,4 +71,4 @@ float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float spee } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt new file mode 100644 index 0000000000000..e6205fa66bf98 --- /dev/null +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_stop_line_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml new file mode 100644 index 0000000000000..8610b7b428bec --- /dev/null +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_velocity_stop_line_module + 0.1.0 + The behavior_velocity_stop_line_module package + + Yutaka Shimizu + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Yutaka Shimizu + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + motion_utils + pluginlib + rclcpp + route_handler + tf2_geometry_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml new file mode 100644 index 0000000000000..51fb225fbebad --- /dev/null +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp rename to planning/behavior_velocity_stop_line_module/src/debug.cpp index d86c6f72b1649..d2b9527b1dd7d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -106,8 +107,7 @@ visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArra *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); if (state_ == State::APPROACH || state_ == State::STOPPED) { appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker( - {p_front}, "stopline", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker({p_front}, "stopline", now), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp rename to planning/behavior_velocity_stop_line_module/src/manager.cpp index 0b647c69dd3d2..86db87ad602cc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "manager.hpp" #include #include @@ -100,4 +100,9 @@ StopLineModuleManager::getModuleExpiredFunction( return stop_line_id_set.count(scene_module->getModuleId()) == 0; }; } + } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp similarity index 83% rename from planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp rename to planning/behavior_velocity_stop_line_module/src/manager.hpp index 68ad08594a70f..4b665a3a536f7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__STOP_LINE__MANAGER_HPP_ -#define SCENE_MODULE__STOP_LINE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -54,6 +57,11 @@ class StopLineModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class StopLineModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__STOP_LINE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_stop_line_module/src/scene.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp rename to planning/behavior_velocity_stop_line_module/src/scene.cpp index 96e306df62fc0..a703f8b5566ad 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include #include -#include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_stop_line_module/src/scene.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp rename to planning/behavior_velocity_stop_line_module/src/scene.hpp index f9846f970beb5..f7540901a5f80 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_ -#define SCENE_MODULE__STOP_LINE__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include #include @@ -24,11 +24,11 @@ #define EIGEN_MPL2_ONLY #include #include +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -111,4 +111,4 @@ class StopLineModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__STOP_LINE__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt new file mode 100644 index 0000000000000..0433c0d0a6c53 --- /dev/null +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_traffic_light_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml new file mode 100644 index 0000000000000..a8733c5dcf5be --- /dev/null +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -0,0 +1,45 @@ + + + + behavior_velocity_traffic_light_module + 0.1.0 + The behavior_velocity_traffic_light_module package + + Satoshi Ota + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..b65cc66c5c232 --- /dev/null +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp rename to planning/behavior_velocity_traffic_light_module/src/debug.cpp index 20363f8633cd0..a57cfe68f1f59 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp rename to planning/behavior_velocity_traffic_light_module/src/manager.cpp index 551fdb6e59425..cad2ed3bda069 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "manager.hpp" #include +#include #include #include #include @@ -202,3 +203,7 @@ bool TrafficLightModuleManager::hasSameTrafficLight( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp similarity index 83% rename from planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp rename to planning/behavior_velocity_traffic_light_module/src/manager.hpp index b93205e5acdb8..450cba09e1e08 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ -#define SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -57,6 +60,11 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC boost::optional first_ref_stop_path_point_index_; }; + +class TrafficLightModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp rename to planning/behavior_velocity_traffic_light_module/src/scene.cpp index b7d345e9d2886..925dfb0470b98 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include #include -#include -#include #include // To be replaced by std::optional in C++17 @@ -27,6 +28,7 @@ #endif #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp rename to planning/behavior_velocity_traffic_light_module/src/scene.hpp index 47b2f8b74d94e..b96c6ebcbb297 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ -#define SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include #include @@ -24,10 +24,10 @@ #define EIGEN_MPL2_ONLY #include #include +#include +#include #include #include -#include -#include #include @@ -148,4 +148,4 @@ class TrafficLightModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt new file mode 100644 index 0000000000000..5e909ebd26177 --- /dev/null +++ b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_virtual_traffic_light_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_virtual_traffic_light_module/package.xml new file mode 100644 index 0000000000000..06b4e7318a791 --- /dev/null +++ b/planning/behavior_velocity_virtual_traffic_light_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_velocity_virtual_traffic_light_module + 0.1.0 + The behavior_velocity_virtual_traffic_light_module package + + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + nlohmann-json-dev + pluginlib + rclcpp + route_handler + tier4_autoware_utils + tier4_planning_msgs + tier4_v2x_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..943ef175aa8f8 --- /dev/null +++ b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp index f60a317de996e..78c22f8873a09 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + #include -#include #include using motion_utils::createStopVirtualWallMarker; @@ -60,7 +61,7 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createVirtualWal if (d.stop_head_pose_at_stop_line) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {*d.stop_head_pose_at_stop_line}, "virtual_traffic_light", now, module_id_), + {*d.stop_head_pose_at_stop_line}, "virtual_traffic_light", now), &wall_marker, now); } @@ -68,7 +69,7 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createVirtualWal if (d.stop_head_pose_at_end_line) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {*d.stop_head_pose_at_end_line}, "virtual_traffic_light", now, module_id_), + {*d.stop_head_pose_at_end_line}, "virtual_traffic_light", now), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp similarity index 93% rename from planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 732e5eb0cb3c7..e5a2b0e43def2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "manager.hpp" + #include #include @@ -72,3 +73,8 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::VirtualTrafficLightModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp index b844bc68ab260..15bd6f468132e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ -#define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -40,6 +43,11 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class VirtualTrafficLightModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp index affc5d7ef8259..fd71635873e7f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include #include -#include -#include -#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 2bd7078d93aeb..bd005fced0a27 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ -#define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ +#include #include #include #include #include #include -#include #include #include @@ -130,4 +130,4 @@ class VirtualTrafficLightModule : public SceneModuleInterface tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index f2741df943c28..feed19fdcc235 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -14,8 +14,7 @@ BSD-3-Clause ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_perception_msgs diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/external_velocity_limit_selector/package.xml index 59456adac97ae..e83966c7b876d 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/external_velocity_limit_selector/package.xml @@ -3,7 +3,7 @@ external_velocity_limit_selector 0.1.0 - The external_velocity_limit_selector ROS2 package + The external_velocity_limit_selector ROS 2 package Satoshi Ota Shinnosuke Hirakawa Shumpei Wakabayashi @@ -15,8 +15,7 @@ Satoshi Ota ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp rclcpp_components diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 1a7e17e78e035..a748b7f80c9ad 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -14,8 +14,7 @@ Tomohito ANDO ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs freespace_planning_algorithms 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 94347f3974aff..8c6336b2f45d6 100644 --- a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,43 +21,74 @@ #include -TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) -{ - rclcpp::init(0, nullptr); +using freespace_planner::FreespacePlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; - auto test_manager = std::make_shared(); +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setRouteInputTopicName("freespace_planner/input/route"); + test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory"); + test_manager->setOdometryTopicName("freespace_planner/input/odometry"); + test_manager->setInitialPoseTopicName("freespace_planner/input/odometry"); + return test_manager; +} +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; - const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto freespace_planner_dir = ament_index_cpp::get_package_share_directory("freespace_planner"); - node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", freespace_planner_dir + "/config/freespace_planner.param.yaml"}); + return std::make_shared(node_options); +} - auto test_target_node = std::make_shared(node_options); - +void publishMandatoryTopics( + std::shared_ptr test_manager, + rclcpp::Node::SharedPtr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); test_manager->publishOdometry(test_target_node, "freespace_planner/input/odometry"); test_manager->publishOccupancyGrid(test_target_node, "freespace_planner/input/occupancy_grid"); test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario"); +} - // set subscriber with topic name: freespace_planner → test_node_ - test_manager->setRouteInputTopicName("freespace_planner/input/route"); +TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) +{ + rclcpp::init(0, nullptr); - // set freespace_planner's input topic name(this topic is changed to test node) - test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory"); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); // test with normal route - ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - test_manager->testWithAbnormalRoute(test_target_node); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } diff --git a/planning/freespace_planning_algorithms/README.md b/planning/freespace_planning_algorithms/README.md index 0084f4c296547..7069a1c0fabfc 100644 --- a/planning/freespace_planning_algorithms/README.md +++ b/planning/freespace_planning_algorithms/README.md @@ -50,6 +50,9 @@ colcon build --packages-select freespace_planning_algorithms colcon test --packages-select freespace_planning_algorithms ``` + + + Inside the test, simulation results are stored in `/tmp/fpalgos-{algorithm_type}-case{scenario_number}` as a rosbag. Loading these resulting files, by using [test/debug_plot.py](test/debug_plot.py), one can create plots visualizing the path and obstacles as shown diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index 690750ac0ab5b..b4e3b2befae40 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -120,6 +120,7 @@ class AbstractPlanningAlgorithm const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape) : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape) { + is_collision_table_initialized = false; } AbstractPlanningAlgorithm( @@ -179,6 +180,9 @@ class AbstractPlanningAlgorithm geometry_msgs::msg::Pose start_pose_; geometry_msgs::msg::Pose goal_pose_; + // Is collision table initalized + bool is_collision_table_initialized; + // result path PlannerWaypoints waypoints_; }; diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 6ba150bb48d92..372e623e101de 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -14,8 +14,7 @@ Hirokazu Ishida ament_cmake_auto - - autoware_cmake + autoware_cmake geometry_msgs nav_msgs diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 2b5814ee2a111..159c0df82fcc3 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -123,11 +123,14 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost is_obstacle_table_ = is_obstacle_table; // construct collision indexes table - for (int i = 0; i < planner_common_param_.theta_size; i++) { - std::vector indexes_2d, vertex_indexes_2d; - computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d); - coll_indexes_table_.push_back(indexes_2d); - vertex_indexes_table_.push_back(vertex_indexes_2d); + if (is_collision_table_initialized == false) { + for (int i = 0; i < planner_common_param_.theta_size; i++) { + std::vector indexes_2d, vertex_indexes_2d; + computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d); + coll_indexes_table_.push_back(indexes_2d); + vertex_indexes_table_.push_back(vertex_indexes_2d); + } + is_collision_table_initialized = true; } } diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 1939908ca22e3..2ea8b96b1cfd2 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -32,6 +32,8 @@ In current Autoware.universe, only Lanelet2 map format is supported. | `/planning/mission_planning/set_route` | autoware_adapi_v1_msgs/srv/SetRoute | route request with lanelet waypoints | | `/planning/mission_planning/change_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route change request with pose waypoints | | `/planning/mission_planning/change_route` | autoware_adapi_v1_msgs/srv/SetRoute | route change request with lanelet waypoints | +| `~/srv/set_mrm_route` | autoware_adapi_v1_msgs/srv/SetRoutePoints | set emergency route | +| `~/srv/clear_mrm_route` | std_srvs/srv/Trigger | clear emergency route | ### Subscriptions @@ -42,12 +44,12 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Publications -| Name | Type | Description | -| ------------------------------- | ------------------------------------- | ------------------------ | -| `/planning/routing/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state | -| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | -| `debug/route_marker` | visualization_msgs/msg/MarkerArray | route marker for debug | -| `debug/goal_footprint` | visualization_msgs/msg/MarkerArray | goal footprint for debug | +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------- | ------------------------ | +| `/planning/mission_planning/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state | +| `/planning/mission_planning/route` | autoware_planning_msgs/LaneletRoute | route | +| `debug/route_marker` | visualization_msgs/msg/MarkerArray | route marker for debug | +| `debug/goal_footprint` | visualization_msgs/msg/MarkerArray | goal footprint for debug | ## Route section diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index a801eca35ce46..fe84be3b2f421 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -6,3 +6,4 @@ arrival_check_duration: 1.0 goal_angle_threshold_deg: 45.0 enable_correct_goal_pose: false + reroute_time_threshold: 10.0 diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp index 4ea9a649a269f..916dabfdbba2b 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -17,11 +17,25 @@ #include +#include #include +#include namespace mission_planner { +struct SetMrmRoute +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; + static constexpr char name[] = "~/srv/set_mrm_route"; +}; + +struct ClearMrmRoute +{ + using Service = std_srvs::srv::Trigger; + static constexpr char name[] = "~/srv/clear_mrm_route"; +}; + struct ModifiedGoal { using Message = autoware_planning_msgs::msg::PoseWithUuidStamped; diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index d71ebefdbeea9..f5f7eefd49d7d 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -15,8 +15,7 @@ Ryohsuke Mitsudome ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_planning_msgs @@ -30,6 +29,7 @@ rclcpp rclcpp_components route_handler + std_srvs tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index cd3def7692940..acb25028b8acf 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -14,10 +14,17 @@ #include "mission_planner.hpp" +#include +#include +#include +#include + #include #include +#include #include +#include #include #include @@ -67,9 +74,12 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) arrival_checker_(this), plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), - tf_listener_(tf_buffer_) + tf_listener_(tf_buffer_), + original_route_(nullptr), + normal_route_(nullptr) { map_frame_ = declare_parameter("map_frame"); + reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); @@ -79,6 +89,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); + auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + vector_map_subscriber_ = create_subscription( + "input/vector_map", qos_transient_local, + std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); @@ -90,6 +105,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points); adaptor.init_srv(srv_change_route_, this, &MissionPlanner::on_change_route); adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); + adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); + adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); change_state(RouteState::Message::UNSET); @@ -110,6 +127,11 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } +void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) +{ + map_ptr_ = msg; +} + PoseStamped MissionPlanner::transform_pose(const PoseStamped & input) { PoseStamped output; @@ -123,10 +145,11 @@ PoseStamped MissionPlanner::transform_pose(const PoseStamped & input) } } -void MissionPlanner::change_route() +void MissionPlanner::clear_route() { arrival_checker_.set_goal(); planner_->clearRoute(); + normal_route_ = nullptr; // TODO(Takagi, Isamu): publish an empty route here } @@ -141,6 +164,9 @@ void MissionPlanner::change_route(const LaneletRoute & route) pub_route_->publish(route); pub_marker_->publish(planner_->visualize(route)); planner_->updateRoute(route); + + // update normal route + normal_route_ = std::make_shared(route); } void MissionPlanner::change_state(RouteState::Message::_state_type state) @@ -154,7 +180,7 @@ void MissionPlanner::change_state(RouteState::Message::_state_type state) void MissionPlanner::on_clear_route( const ClearRoute::Service::Request::SharedPtr, const ClearRoute::Service::Response::SharedPtr res) { - change_route(); + clear_route(); change_state(RouteState::Message::UNSET); res->status.success = true; } @@ -194,6 +220,7 @@ void MissionPlanner::on_set_route( // Update route. change_route(route); change_state(RouteState::Message::SET); + original_route_ = std::make_shared(route); res->status.success = true; } @@ -245,10 +272,30 @@ void MissionPlanner::on_set_route_points( // Update route. change_route(route); change_state(RouteState::Message::SET); + original_route_ = std::make_shared(route); res->status.success = true; } // NOTE: The route interface should be mutually exclusive by callback group. +void MissionPlanner::on_set_mrm_route( + const SetMrmRoute::Service::Request::SharedPtr req, + const SetMrmRoute::Service::Response::SharedPtr res) +{ + // TODO(Yutaka Shimizu): reroute for MRM + (void)req; + (void)res; +} + +// NOTE: The route interface should be mutually exclusive by callback group. +void MissionPlanner::on_clear_mrm_route( + const ClearMrmRoute::Service::Request::SharedPtr req, + const ClearMrmRoute::Service::Response::SharedPtr res) +{ + // TODO(Yutaka Shimizu): reroute for MRM + (void)req; + (void)res; +} + void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) { // TODO(Yutaka Shimizu): reroute if the goal is outside the lane. @@ -258,9 +305,50 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt void MissionPlanner::on_change_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res) { - // TODO(Yutaka Shimizu): reroute - (void)req; - (void)res; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + + if (state_.state != RouteState::Message::SET) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); + } + if (!odometry_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + + change_state(RouteState::Message::CHANGING); + + // Use temporary pose stamped for transform. + PoseStamped pose; + pose.header = req->header; + pose.pose = req->goal; + + // Convert route. + LaneletRoute new_route; + new_route.start_pose = odometry_->pose.pose; + new_route.goal_pose = transform_pose(pose).pose; + for (const auto & segment : req->segments) { + new_route.segments.push_back(convert(segment)); + } + new_route.header.stamp = req->header.stamp; + new_route.header.frame_id = map_frame_; + new_route.uuid.uuid = generate_random_id(); + new_route.allow_modification = req->option.allow_goal_modification; + + // check route safety + if (checkRerouteSafety(*normal_route_, new_route)) { + // sucess to reroute + change_route(new_route); + change_state(RouteState::Message::SET); + res->status.success = true; + } else { + // failed to reroute + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->status.success = false; + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); + } } // NOTE: The route interface should be mutually exclusive by callback group. @@ -268,11 +356,167 @@ void MissionPlanner::on_change_route_points( const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res) { - // TODO(Yutaka Shimizu): reroute - (void)req; - (void)res; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + + if (state_.state != RouteState::Message::SET) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); + } + if (!planner_->ready()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } + if (!odometry_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + + change_state(RouteState::Message::CHANGING); + + // Use temporary pose stamped for transform. + PoseStamped pose; + pose.header = req->header; + + // Convert route points. + PlannerPlugin::RoutePoints points; + points.push_back(odometry_->pose.pose); + for (const auto & waypoint : req->waypoints) { + pose.pose = waypoint; + points.push_back(transform_pose(pose).pose); + } + pose.pose = req->goal; + points.push_back(transform_pose(pose).pose); + + // Plan route. + LaneletRoute new_route = planner_->plan(points); + if (new_route.segments.empty()) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + new_route.header.stamp = req->header.stamp; + new_route.header.frame_id = map_frame_; + new_route.uuid.uuid = generate_random_id(); + new_route.allow_modification = req->option.allow_goal_modification; + + // check route safety + if (checkRerouteSafety(*normal_route_, new_route)) { + // sucess to reroute + change_route(new_route); + change_state(RouteState::Message::SET); + res->status.success = true; + } else { + // failed to reroute + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->status.success = false; + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); + } } +bool MissionPlanner::checkRerouteSafety( + const LaneletRoute & original_route, const LaneletRoute & target_route) +{ + if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) { + return false; + } + + // find the index of the original route that has same idx with the front segment of the new route + const auto target_front_primitives = target_route.segments.front().primitives; + + auto hasSamePrimitives = []( + const std::vector & original_primitives, + const std::vector & target_primitives) { + if (original_primitives.size() != target_primitives.size()) { + return false; + } + + bool is_same = false; + for (const auto & primitive : original_primitives) { + const auto has_same = [&](const auto & p) { return p.id == primitive.id; }; + is_same = std::find_if(target_primitives.begin(), target_primitives.end(), has_same) != + target_primitives.end(); + } + return is_same; + }; + + // find idx that matches the target primitives + size_t start_idx = original_route.segments.size(); + for (size_t i = 0; i < original_route.segments.size(); ++i) { + const auto & original_primitives = original_route.segments.at(i).primitives; + if (hasSamePrimitives(original_primitives, target_front_primitives)) { + start_idx = i; + break; + } + } + + // find last idx that matches the target primitives + size_t end_idx = start_idx; + for (size_t i = 1; i < target_route.segments.size(); ++i) { + if (start_idx + i > original_route.segments.size() - 1) { + break; + } + + const auto & original_primitives = original_route.segments.at(start_idx + i).primitives; + const auto & target_primitives = target_route.segments.at(i).primitives; + if (!hasSamePrimitives(original_primitives, target_primitives)) { + break; + } + end_idx = start_idx + i; + } + + // create map + auto lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*map_ptr_, lanelet_map_ptr_); + + // compute distance from the current pose to the end of the current lanelet + const auto current_pose = target_route.start_pose; + const auto primitives = original_route.segments.at(start_idx).primitives; + lanelet::ConstLanelets start_lanelets; + for (const auto & primitive : primitives) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id); + start_lanelets.push_back(lanelet); + } + + // get closest lanelet in start lanelets + lanelet::ConstLanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { + return false; + } + + const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline()); + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + const double dist_to_current_pose = arc_coordinates.length; + const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet); + double accumulated_length = lanelet_length - dist_to_current_pose; + + // compute distance from the start_idx+1 to end_idx + for (size_t i = start_idx + 1; i <= end_idx; ++i) { + const auto primitives = original_route.segments.at(i).primitives; + if (primitives.empty()) { + break; + } + + std::vector lanelets_length(primitives.size()); + for (size_t primitive_idx = 0; primitive_idx < primitives.size(); ++primitive_idx) { + const auto & primitive = primitives.at(primitive_idx); + const auto & lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id); + lanelets_length.at(primitive_idx) = (lanelet::utils::getLaneletLength2d(lanelet)); + } + accumulated_length += *std::min_element(lanelets_length.begin(), lanelets_length.end()); + } + + // check safety + const auto current_velocity = odometry_->twist.twist.linear.x; + if (accumulated_length > current_velocity * reroute_time_threshold_) { + return true; + } + + return false; +} } // namespace mission_planner #include diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 7e8bcdf11babe..1c554ca339958 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -41,6 +42,8 @@ namespace mission_planner using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; +using LaneletPrimitive = autoware_planning_msgs::msg::LaneletPrimitive; +using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using MarkerArray = visualization_msgs::msg::MarkerArray; using ClearRoute = planning_interface::ClearRoute; using SetRoutePoints = planning_interface::SetRoutePoints; @@ -66,11 +69,12 @@ class MissionPlanner : public rclcpp::Node PoseStamped transform_pose(const PoseStamped & input); rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr vector_map_subscriber_; Odometry::ConstSharedPtr odometry_; void on_odometry(const Odometry::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; - void change_route(); + void clear_route(); void change_route(const LaneletRoute & route); RouteState::Message state_; @@ -93,6 +97,18 @@ class MissionPlanner : public rclcpp::Node const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res); + component_interface_utils::Service::SharedPtr srv_set_mrm_route_; + component_interface_utils::Service::SharedPtr srv_clear_mrm_route_; + void on_set_mrm_route( + const SetMrmRoute::Service::Request::SharedPtr req, + const SetMrmRoute::Service::Response::SharedPtr res); + void on_clear_mrm_route( + const ClearMrmRoute::Service::Request::SharedPtr req, + const ClearMrmRoute::Service::Response::SharedPtr res); + + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + void onMap(const HADMapBin::ConstSharedPtr msg); + component_interface_utils::Subscription::SharedPtr sub_modified_goal_; void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( @@ -101,6 +117,12 @@ class MissionPlanner : public rclcpp::Node void on_change_route_points( const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res); + + double reroute_time_threshold_{10.0}; + bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route); + + std::shared_ptr original_route_{nullptr}; + std::shared_ptr normal_route_{nullptr}; }; } // namespace mission_planner diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt index b69cf0d660a66..7fd9c5681315c 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/motion_velocity_smoother/CMakeLists.txt @@ -52,12 +52,12 @@ if(BUILD_TESTING) target_link_libraries(test_smoother_functions smoother ) - # ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - # test/test_motion_velocity_smoother_node_interface.cpp - # ) - # target_link_libraries(test_${PROJECT_NAME} - # motion_velocity_smoother_node - # ) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_motion_velocity_smoother_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + motion_velocity_smoother_node + ) endif() diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index dcbdbf8128b61..f19ead1874f21 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -17,10 +17,9 @@ Makoto Kurihara ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake - autoware_auto_planning_msgs geometry_msgs interpolation diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 8531fec2c5764..357f5a9ada3d8 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -402,6 +402,16 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar return; } + // calculate trajectory velocity + auto input_points = motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); + + // guard for invalid trajectory + input_points = motion_utils::removeOverlapPoints(input_points); + if (input_points.size() < 2) { + RCLCPP_ERROR(get_logger(), "No enough points in trajectory after overlap points removal"); + return; + } + // calculate prev closest point if (!prev_output_.empty()) { current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); @@ -414,9 +424,6 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // ignore current external velocity limit next time external_velocity_limit_ptr_ = nullptr; - // calculate trajectory velocity - auto input_points = motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); - // For negative velocity handling, multiple -1 to velocity if it is for reverse. // NOTE: this process must be in the beginning of the process is_reverse_ = isReverse(input_points); @@ -819,19 +826,23 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( 0, traj.size(), max_velocity_with_deceleration_, traj); - const size_t closest_idx = findNearestIndexFromEgo(traj); - - double dist = 0.0; - for (size_t idx = closest_idx; idx < traj.size() - 1; ++idx) { - dist += tier4_autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); - if (dist > external_velocity_limit_.dist) { - trajectory_utils::applyMaximumVelocityLimit( - idx + 1, traj.size(), external_velocity_limit_.velocity, traj); - return; - } + // insert the point at the distance of external velocity limit + const auto & current_pose = current_odometry_ptr_->pose.pose; + const size_t closest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto inserted_index = + motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); + if (!inserted_index) { + traj.back().longitudinal_velocity_mps = std::min( + traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_.velocity)); + return; } - traj.back().longitudinal_velocity_mps = std::min( - traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_.velocity)); + + // apply external velocity limit from the inserted point + trajectory_utils::applyMaximumVelocityLimit( + *inserted_index, traj.size(), external_velocity_limit_.velocity, traj); + RCLCPP_DEBUG( get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); } diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 8a364f876b329..6c2bef3ae08c4 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -14,9 +14,10 @@ #include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "eigen3/Eigen/Core" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include + #include #include #include @@ -293,6 +294,17 @@ bool JerkFilteredSmoother::apply( // execute optimization const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); const std::vector optval = std::get<0>(result); + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } const auto tf1 = std::chrono::system_clock::now(); const double dt_ms1 = @@ -317,7 +329,7 @@ bool JerkFilteredSmoother::apply( const auto msg = status_polish == 0 ? "unperformed" : status_polish == -1 ? "unsuccessful" : "unknown"; - RCLCPP_WARN(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg); + RCLCPP_DEBUG(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg); } if (VERBOSE_TRAJECTORY_VELOCITY) { diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index f80fc0231e9cd..d46deeeeb9f10 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -14,9 +14,10 @@ #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "eigen3/Eigen/Core" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include + #include #include #include @@ -189,6 +190,17 @@ bool L2PseudoJerkSmoother::apply( // [b0, b1, ..., bN, | a0, a1, ..., aN, | // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] const std::vector optval = std::get<0>(result); + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } for (unsigned int i = 0; i < N; ++i) { double v = optval.at(i); diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 7c3fe6e0ceed2..e8900b1947454 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -14,9 +14,10 @@ #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "eigen3/Eigen/Core" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include + #include #include #include @@ -204,6 +205,17 @@ bool LinfPseudoJerkSmoother::apply( // [b0, b1, ..., bN, | a0, a1, ..., aN, | // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] const std::vector optval = std::get<0>(result); + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } /* get velocity & acceleration */ for (unsigned int i = 0; i < N; ++i) { 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 dec629c22818e..d9577a5c62814 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -28,18 +28,18 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu auto test_manager = std::make_shared(); auto node_options = rclcpp::NodeOptions{}; - - test_manager->declareVehicleInfoParams(node_options); - test_manager->declareNearestSearchDistanceParams(node_options); node_options.append_parameter_override("algorithm_type", "JerkFiltered"); node_options.append_parameter_override("publish_debug_trajs", false); - + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); - node_options.arguments( - {"--ros-args", "--params-file", - motion_velocity_smoother_dir + "/config/test_default_motion_velocity_smoother.param.yaml", + {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", + "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + "--params-file", + motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", "--params-file", motion_velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", motion_velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); @@ -62,5 +62,5 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - test_manager->testWithAbnormalTrajectory(test_target_node); + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); } diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index c851de40aa9ec..883ce473722fd 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -46,6 +46,15 @@ rclcpp_components_register_node(obstacle_avoidance_planner EXECUTABLE obstacle_avoidance_planner_node ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_${PROJECT_NAME}_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch @@ -54,6 +63,6 @@ ament_auto_package( ) install(PROGRAMS - scripts/calclation_time_plotter.py + scripts/calculation_time_plotter.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/obstacle_avoidance_planner/README.md index 200e1f9d26eb0..36a692ca33735 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/obstacle_avoidance_planner/README.md @@ -1,3 +1,5 @@ +# Obstacle Avoidance Planner + ## Purpose This package generates a trajectory that is kinematically-feasible to drive and collision-free based on the input path, drivable area. diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 5586de985d8d7..3f4655b78229c 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -23,6 +23,8 @@ # replanning & trimming trajectory param outside algorithm replan: max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_path_shape_forward_lon_dist: 100.0 # forward point to check lateral distance difference [m] + max_path_shape_forward_lat_dist: 0.1 # threshold of path shape change around forward point [m] max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] # make max_goal_moving_dist long to keep start point fixed for pull over max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] @@ -102,11 +104,13 @@ # avoidance avoidance: + max_bound_fixing_time: 3.0 # [s] max_longitudinal_margin_for_bound_violation: 1.0 # [m] max_avoidance_cost: 0.5 # [m] avoidance_cost_margin: 0.0 # [m] avoidance_cost_band_length: 5.0 # [m] avoidance_cost_decrease_rate: 0.05 # decreased cost per point interval + min_drivable_width: 0.2 # [m] The vehicle width and this parameter is guaranteed to keep for collision free constraint. weight: lat_error_weight: 0.0 # weight for lateral error diff --git a/planning/obstacle_avoidance_planner/docs/debug.md b/planning/obstacle_avoidance_planner/docs/debug.md index 9ee18b732d375..f5bcf56a6138a 100644 --- a/planning/obstacle_avoidance_planner/docs/debug.md +++ b/planning/obstacle_avoidance_planner/docs/debug.md @@ -28,7 +28,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. ![path_footprint](../media/debug/path_footprint_visualization.png) -- **Drivalbe Area** +- **Drivable Area** - The Drivable area generated in the `behavior` planner. - The skyblue left and right line strings, that is visualized by default. - NOTE: @@ -69,7 +69,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. - **Vehicle Circles** - The vehicle's shape is represented by a set of circles. - - The `obstcle_avoidance_planner` will try to make the these circles inside the above boundaries' width. + - The `obstacle_avoidance_planner` will try to make the these circles inside the above boundaries' width. ![vehicle_circles](../media/debug/vehicle_circles_visualization.png) @@ -147,7 +147,7 @@ onPath:= 20.737 [ms] With the following script, any calculation time of the above functions can be plot. ```sh -ros2 run obstacle_avoidance_planner calclation_time_plotter.py +ros2 run obstacle_avoidance_planner calculation_time_plotter.py ``` ![calculation_time_plot](../media/debug/calculation_time_plot.png) @@ -155,7 +155,7 @@ ros2 run obstacle_avoidance_planner calclation_time_plotter.py You can specify functions to plot with the `-f` option. ```sh -ros2 run obstacle_avoidance_planner calclation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" +ros2 run obstacle_avoidance_planner calculation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" ``` ## Q&A for Debug diff --git a/planning/obstacle_avoidance_planner/docs/mpt.md b/planning/obstacle_avoidance_planner/docs/mpt.md index f76115b8da1cb..5bc2ddefe10c6 100644 --- a/planning/obstacle_avoidance_planner/docs/mpt.md +++ b/planning/obstacle_avoidance_planner/docs/mpt.md @@ -360,7 +360,7 @@ $$ To realize collision-free trajectory planning, we have to formulate constraints that the vehicle is inside the road and also does not collide with obstacles in linear equations. For linearity, we implemented some methods to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. -- 1. Bibycle Model +- 1. Bicycle Model - 2. Uniform Circles - 3. Fitting Uniform Circles diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp index e4ac55dba5f4e..24a419c8bef66 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp @@ -15,11 +15,12 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ -#include "eigen3/Eigen/Core" #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" +#include + #include #include #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index eaffa77cb7826..b68c54ae51f2f 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -15,8 +15,6 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Sparse" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" @@ -27,6 +25,9 @@ #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + #include #include #include @@ -178,11 +179,13 @@ class MPTOptimizer double steer_rate_weight; // avoidance + double max_bound_fixing_time; double max_longitudinal_margin_for_bound_violation; double max_avoidance_cost; double avoidance_cost_margin; double avoidance_cost_band_length; double avoidance_cost_decrease_rate; + double min_drivable_width; double avoidance_lat_error_weight; double avoidance_yaw_error_weight; double avoidance_steer_input_weight; @@ -248,9 +251,14 @@ class MPTOptimizer void updateBounds( std::vector & ref_points, const std::vector & left_bound, - const std::vector & right_bound) const; + const std::vector & right_bound, + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const; + void keepMinimumBoundsWidth(std::vector & ref_points) const; std::vector extendViolatedBounds( const std::vector & ref_points) const; + void avoidSuddenSteering( + std::vector & ref_points, const geometry_msgs::msg::Pose & ego_pose, + const double ego_vel) const; void updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp index 6b456a6757626..1e4699556ef24 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp @@ -32,9 +32,13 @@ class ReplanChecker explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); void onParam(const std::vector & parameters); - bool isResetRequired(const PlannerData & planner_data); + bool isResetRequired(const PlannerData & planner_data) const; - bool isReplanRequired(const rclcpp::Time & current_time); + bool isReplanRequired(const PlannerData & planner_data, const rclcpp::Time & current_time) const; + + void updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time); private: EgoNearestParam ego_nearest_param_; @@ -49,12 +53,16 @@ class ReplanChecker // algorithm parameters double max_path_shape_around_ego_lat_dist_; + double max_path_shape_forward_lat_dist_; + double max_path_shape_forward_lon_dist_; double max_ego_moving_dist_; double max_goal_moving_dist_; double max_delta_time_sec_; bool isPathAroundEgoChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp index 6673058ba59a2..1ec61f583f661 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp @@ -15,7 +15,6 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ -#include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" @@ -24,6 +23,8 @@ #include "obstacle_avoidance_planner/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include + #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp index 3a2d584911994..efc75f4c90bba 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -15,7 +15,6 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ -#include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" @@ -23,6 +22,8 @@ #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" +#include + #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -53,7 +54,7 @@ namespace trajectory_utils template std::optional getPointIndexAfter( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double offset) + const double max_offset, const double min_offset) { if (points.empty()) { return std::nullopt; @@ -62,28 +63,35 @@ std::optional getPointIndexAfter( double sum_length = -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + std::optional output_idx{std::nullopt}; + // search forward - if (sum_length < offset) { + if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); - if (offset < sum_length) { - return i - 1; + if (min_offset < sum_length) { + output_idx = i - 1; + } + if (max_offset < sum_length) { + break; } } - - return std::nullopt; + return output_idx; } // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); - if (sum_length < offset) { - return i - 1; + if (sum_length < min_offset) { + output_idx = i - 1; + } + if (sum_length < max_offset) { + break; } } - return 0; + return output_idx; } template diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 13fd2995befb0..f892b0b8d13de 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -15,10 +15,11 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include +#include + #include class KinematicsBicycleModel : public VehicleModelInterface diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp index 77dbaa80c7bbe..3fd0129315edd 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Sparse" +#include +#include #include diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index 6aa659ff60085..81429368fc7e4 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -12,8 +12,7 @@ Takayuki Murooka ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_planning_msgs geometry_msgs @@ -21,6 +20,7 @@ motion_utils nav_msgs osqp_interface + planning_test_utils rclcpp rclcpp_components std_msgs diff --git a/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py similarity index 97% rename from planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py rename to planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py index f0d32848adefe..042c2ff5f0345 100755 --- a/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py +++ b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py @@ -70,8 +70,6 @@ def CallbackCalculationCost(self, msg): if len(self.y_vec[f_idx]) > 100: self.y_vec[f_idx].popleft() - print(len(self.y_vec[f_idx])) - x_vec = list(range(len(self.y_vec[f_idx]))) valid_x_vec = [] @@ -81,8 +79,6 @@ def CallbackCalculationCost(self, msg): valid_x_vec.append(x_vec[i]) valid_y_vec.append(self.y_vec[f_idx][i]) - print(len(valid_x_vec), len(valid_y_vec)) - self.lines[f_idx].set_xdata(valid_x_vec) self.lines[f_idx].set_ydata(valid_y_vec) diff --git a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp index a988f78c9074e..31af863eed8f0 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp @@ -380,6 +380,12 @@ std::optional> EBPathSmoother::optimizeTrajectory() osqp_solver_ptr_->logUnsolvedStatus("[EB]"); return std::nullopt; } + const auto has_nan = std::any_of( + optimized_points.begin(), optimized_points.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return std::nullopt; + } time_keeper_ptr_->toc(__func__, " "); return optimized_points; diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 565ef893facaf..439b68e9032dd 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -239,12 +239,14 @@ MPTOptimizer::MPTParam::MPTParam( { // avoidance max_longitudinal_margin_for_bound_violation = node->declare_parameter("mpt.avoidance.max_longitudinal_margin_for_bound_violation"); + max_bound_fixing_time = node->declare_parameter("mpt.avoidance.max_bound_fixing_time"); max_avoidance_cost = node->declare_parameter("mpt.avoidance.max_avoidance_cost"); avoidance_cost_margin = node->declare_parameter("mpt.avoidance.avoidance_cost_margin"); avoidance_cost_band_length = node->declare_parameter("mpt.avoidance.avoidance_cost_band_length"); avoidance_cost_decrease_rate = node->declare_parameter("mpt.avoidance.avoidance_cost_decrease_rate"); + min_drivable_width = node->declare_parameter("mpt.avoidance.min_drivable_width"); avoidance_lat_error_weight = node->declare_parameter("mpt.avoidance.weight.lat_error_weight"); @@ -382,6 +384,8 @@ void MPTOptimizer::MPTParam::onParam(const std::vector & para updateParam( parameters, "mpt.avoidance.max_longitudinal_margin_for_bound_violation", max_longitudinal_margin_for_bound_violation); + updateParam(parameters, "mpt.avoidance.max_bound_fixing_time", max_bound_fixing_time); + updateParam(parameters, "mpt.avoidance.min_drivable_width", min_drivable_width); updateParam(parameters, "mpt.avoidance.max_avoidance_cost", max_avoidance_cost); updateParam(parameters, "mpt.avoidance.avoidance_cost_margin", avoidance_cost_margin); updateParam( @@ -585,7 +589,7 @@ std::vector MPTOptimizer::calcReferencePoints( // 6. update bounds // NOTE: After this, resample must not be called since bounds are not interpolated. - updateBounds(ref_points, p.left_bound, p.right_bound); + updateBounds(ref_points, p.left_bound, p.right_bound, p.ego_pose, p.ego_vel); updateVehicleBounds(ref_points, ref_points_spline); // 7. update delta arc length @@ -780,7 +784,8 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c void MPTOptimizer::updateBounds( std::vector & ref_points, const std::vector & left_bound, - const std::vector & right_bound) const + const std::vector & right_bound, + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { time_keeper_ptr_->tic(__func__); @@ -799,13 +804,21 @@ void MPTOptimizer::updateBounds( ref_point_for_bound_search.pose, left_bound, soft_road_clearance, true); const double dist_to_right_bound = calcLateralDistToBounds( ref_point_for_bound_search.pose, right_bound, soft_road_clearance, false); - ref_points.at(i).bounds = Bounds{dist_to_right_bound, dist_to_left_bound}; } + // keep vehicle width + margin + // NOTE: The drivable area's width is sometimes narrower than the vehicle width which means + // infeasible to run especially when obstacles are extracted from the drivable area. + // In this case, the drivable area's width is forced to be wider. + keepMinimumBoundsWidth(ref_points); + // extend violated bounds, where the input path is outside the drivable area ref_points = extendViolatedBounds(ref_points); + // keep previous boundary's width around ego to avoid sudden steering + avoidSuddenSteering(ref_points, ego_pose, ego_vel); + /* // TODO(murooka) deal with filling data between obstacles // fill between obstacles @@ -827,6 +840,160 @@ void MPTOptimizer::updateBounds( return; } +void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_points) const +{ + // 1. calculate start and end sections which are out of bounds + std::vector> out_of_upper_bound_sections; + std::vector> out_of_lower_bound_sections; + std::optional out_of_upper_bound_start_idx = std::nullopt; + std::optional out_of_lower_bound_start_idx = std::nullopt; + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto & b = ref_points.at(i).bounds; + // const double drivable_width = b.upper_bound - b.lower_bound; + // const bool is_infeasible_to_drive = drivable_width < mpt_param_.min_drivable_width; + + // NOTE: The following condition should be uncommented to see obstacles outside the path. + // However, on a narrow road, the ego may go outside the road border with this condition. + // Currently, we cannot distinguish obstacles and road border + if (/*is_infeasible_to_drive ||*/ b.upper_bound < 0.0) { // out of upper bound + if (!out_of_upper_bound_start_idx) { + out_of_upper_bound_start_idx = i; + } + } else { + if (out_of_upper_bound_start_idx) { + out_of_upper_bound_sections.push_back({*out_of_upper_bound_start_idx, i - 1}); + out_of_upper_bound_start_idx = std::nullopt; + } + } + if (/*is_infeasible_to_drive ||*/ 0.0 < b.lower_bound) { // out of lower bound + if (!out_of_lower_bound_start_idx) { + out_of_lower_bound_start_idx = i; + } + } else { + if (out_of_lower_bound_start_idx) { + out_of_lower_bound_sections.push_back({*out_of_lower_bound_start_idx, i - 1}); + out_of_lower_bound_start_idx = std::nullopt; + } + } + } + if (out_of_upper_bound_start_idx) { + out_of_upper_bound_sections.push_back({*out_of_upper_bound_start_idx, ref_points.size() - 1}); + } + if (out_of_lower_bound_start_idx) { + out_of_lower_bound_sections.push_back({*out_of_lower_bound_start_idx, ref_points.size() - 1}); + } + + auto original_ref_points = ref_points; + const auto is_inside_sections = [&](const size_t target_idx, const auto & sections) { + for (const auto & section : sections) { + if (section.first <= target_idx && target_idx <= section.second) { + return true; + } + } + return false; + }; + + // lower bound + for (const auto & out_of_lower_bound_section : out_of_lower_bound_sections) { + std::optional upper_bound_start_idx = std::nullopt; + std::optional upper_bound_end_idx = std::nullopt; + for (size_t p_idx = out_of_lower_bound_section.first; + p_idx <= out_of_lower_bound_section.second; ++p_idx) { + const bool is_out_of_upper_bound = is_inside_sections(p_idx, out_of_upper_bound_sections); + + const auto & original_b = original_ref_points.at(p_idx).bounds; + auto & b = ref_points.at(p_idx).bounds; + if (is_out_of_upper_bound) { + if (!upper_bound_start_idx) { + upper_bound_start_idx = p_idx; + } + upper_bound_end_idx = p_idx; + + // It seems both bounds are cut out. Widen the bounds towards the both side. + const double center_dist_to_bounds = + (original_b.upper_bound + original_b.lower_bound) / 2.0; + b.upper_bound = + std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0); + b.lower_bound = + std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0); + continue; + } + // Only the Lower bound is cut out. Widen the bounds towards the lower bound since cut out too + // much. + b.lower_bound = + std::min(b.lower_bound, original_b.upper_bound - mpt_param_.min_drivable_width); + continue; + } + // extend longitudinal if it overlaps out_of_upper_bound_sections + if (upper_bound_start_idx) { + for (size_t p_idx = out_of_lower_bound_section.first; p_idx < *upper_bound_start_idx; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.lower_bound = + std::min(b.lower_bound, ref_points.at(*upper_bound_start_idx).bounds.lower_bound); + } + } + if (upper_bound_end_idx) { + for (size_t p_idx = *upper_bound_end_idx + 1; p_idx <= out_of_lower_bound_section.second; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.lower_bound = + std::min(b.lower_bound, ref_points.at(*upper_bound_end_idx).bounds.lower_bound); + } + } + } + + // upper bound + for (const auto & out_of_upper_bound_section : out_of_upper_bound_sections) { + std::optional lower_bound_start_idx = std::nullopt; + std::optional lower_bound_end_idx = std::nullopt; + for (size_t p_idx = out_of_upper_bound_section.first; + p_idx <= out_of_upper_bound_section.second; ++p_idx) { + const bool is_out_of_lower_bound = is_inside_sections(p_idx, out_of_lower_bound_sections); + + const auto & original_b = original_ref_points.at(p_idx).bounds; + auto & b = ref_points.at(p_idx).bounds; + if (is_out_of_lower_bound) { + if (!lower_bound_start_idx) { + lower_bound_start_idx = p_idx; + } + lower_bound_end_idx = p_idx; + + // It seems both bounds are cut out. Widen the bounds towards the both side. + const double center_dist_to_bounds = + (original_b.upper_bound + original_b.lower_bound) / 2.0; + b.upper_bound = + std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0); + b.lower_bound = + std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0); + continue; + } + // Only the Upper bound is cut out. Widen the bounds towards the upper bound since cut out too + // much. + b.upper_bound = + std::max(b.upper_bound, original_b.lower_bound + mpt_param_.min_drivable_width); + continue; + } + // extend longitudinal if it overlaps out_of_lower_bound_sections + if (lower_bound_start_idx) { + for (size_t p_idx = out_of_upper_bound_section.first; p_idx < *lower_bound_start_idx; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.upper_bound = + std::max(b.upper_bound, ref_points.at(*lower_bound_start_idx).bounds.upper_bound); + } + } + if (lower_bound_end_idx) { + for (size_t p_idx = *lower_bound_end_idx + 1; p_idx <= out_of_upper_bound_section.second; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.upper_bound = + std::max(b.upper_bound, ref_points.at(*lower_bound_end_idx).bounds.upper_bound); + } + } + } +} + std::vector MPTOptimizer::extendViolatedBounds( const std::vector & ref_points) const { @@ -874,6 +1041,34 @@ std::vector MPTOptimizer::extendViolatedBounds( return extended_ref_points; } +void MPTOptimizer::avoidSuddenSteering( + std::vector & ref_points, const geometry_msgs::msg::Pose & ego_pose, + const double ego_vel) const +{ + if (!prev_ref_points_ptr_) { + return; + } + const size_t prev_ego_idx = trajectory_utils::findEgoIndex( + *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.front()), ego_nearest_param_); + + const double max_bound_fixing_length = ego_vel * mpt_param_.max_bound_fixing_time; + const int max_bound_fixing_idx = + std::floor(max_bound_fixing_length / mpt_param_.delta_arc_length); + + const size_t ego_idx = trajectory_utils::findEgoIndex(ref_points, ego_pose, ego_nearest_param_); + const size_t max_fixed_bound_idx = + std::min(ego_idx + static_cast(max_bound_fixing_idx), ref_points.size()); + + for (size_t i = 0; i < max_fixed_bound_idx; ++i) { + const size_t prev_idx = std::min( + prev_ego_idx + i, static_cast(static_cast(prev_ref_points_ptr_->size()) - 1)); + const auto & prev_bounds = prev_ref_points_ptr_->at(prev_idx).bounds; + + ref_points.at(i).bounds.upper_bound = prev_bounds.upper_bound; + ref_points.at(i).bounds.lower_bound = prev_bounds.lower_bound; + } +} + void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const @@ -1345,6 +1540,13 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // get optimization result auto optimization_result = std::get<0>(result); // NOTE: const cannot be added due to the next operation. + const auto has_nan = std::any_of( + optimization_result.begin(), optimization_result.end(), + [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return std::nullopt; + } const Eigen::VectorXd optimized_steer_angles = Eigen::Map(&optimization_result[0], D_un); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 68751f98f1be8..25b65b92c0924 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -314,16 +314,19 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( const auto & p = planner_data; // 1. check if replan (= optimization) is required - const bool reset_prev_optimization = replan_checker_ptr_->isResetRequired(planner_data); - if (enable_reset_prev_optimization_ || reset_prev_optimization) { - // NOTE: always replan when resetting previous optimization - resetPreviousData(); - } else { - // check replan when not resetting previous optimization - const bool is_replan_required = replan_checker_ptr_->isReplanRequired(now()); - if (!is_replan_required) { - return getPrevOptimizedTrajectory(p.traj_points); + const bool is_replan_required = [&]() { + const bool reset_prev_optimization = replan_checker_ptr_->isResetRequired(planner_data); + if (enable_reset_prev_optimization_ || reset_prev_optimization) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + return true; } + // check replan when not resetting previous optimization + return replan_checker_ptr_->isReplanRequired(planner_data, now()); + }(); + replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); + if (!is_replan_required) { + return getPrevOptimizedTrajectory(p.traj_points); } if (enable_skip_optimization_) { @@ -509,26 +512,37 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( trajectory_utils::findEgoSegmentIndex(traj_points, joint_start_pose, ego_nearest_param_); // crop trajectory for extension - constexpr double joint_traj_length_for_smoothing = 5.0; + constexpr double joint_traj_max_length_for_smoothing = 15.0; + constexpr double joint_traj_min_length_for_smoothing = 5.0; const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, - joint_traj_length_for_smoothing); + joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); // calculate full trajectory points const auto full_traj_points = [&]() { if (!joint_end_traj_point_idx) { return optimized_traj_points; } + const auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - return concatVectors(optimized_traj_points, extended_traj_points); + + // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is + // zero velocity, the zero velocity will be inserted in the whole joint trajectory. + auto modified_optimized_traj_points = optimized_traj_points; + if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { + modified_optimized_traj_points.back().longitudinal_velocity_mps = + extended_traj_points.front().longitudinal_velocity_mps; + } + + return concatVectors(modified_optimized_traj_points, extended_traj_points); }(); // resample trajectory points auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints( full_traj_points, traj_param_.output_delta_arc_length); - // update velocity on joint + // update stop velocity on joint for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp index 42ebab7f14e5d..8d281ced9a5ac 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -27,6 +27,10 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne { max_path_shape_around_ego_lat_dist_ = node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_path_shape_forward_lat_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lat_dist"); + max_path_shape_forward_lon_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lon_dist"); max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); @@ -38,12 +42,16 @@ void ReplanChecker::onParam(const std::vector & parameters) updateParam( parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); } -bool ReplanChecker::isResetRequired(const PlannerData & planner_data) +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const { const auto & p = planner_data; @@ -56,7 +64,8 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) // path shape changes if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { - RCLCPP_INFO(logger_, "Replan with resetting optimization since path shape changed."); + RCLCPP_INFO( + logger_, "Replan with resetting optimization since path shape around ego changed."); return true; } @@ -79,20 +88,18 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) return false; }(); - // update previous information required in this function - prev_traj_points_ptr_ = std::make_shared>(p.traj_points); - prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); - return reset_required; } -bool ReplanChecker::isReplanRequired(const rclcpp::Time & current_time) +bool ReplanChecker::isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time) const { const bool replan_required = [&]() { // guard for invalid variables if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) { return true; } + const auto & prev_traj_points = *prev_traj_points_ptr_; // time elapses const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); @@ -100,15 +107,32 @@ bool ReplanChecker::isReplanRequired(const rclcpp::Time & current_time) return true; } + // path shape changes + if (isPathForwardChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan since path forward shape changed."); + return true; + } + return false; }(); + return replan_required; +} + +void ReplanChecker::updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time) +{ + const auto & p = planner_data; + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + // update previous information required in this function - if (replan_required) { + if (is_replan_required) { prev_replanned_time_ptr_ = std::make_shared(current_time); } - - return replan_required; } bool ReplanChecker::isPathAroundEgoChanged( @@ -136,6 +160,38 @@ bool ReplanChecker::isPathAroundEgoChanged( return true; } +bool ReplanChecker::isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate forward point of previous trajectory points + const size_t prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + + // check if distance is larger than the threshold + constexpr double lon_dist_interval = 10.0; + for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; + lon_dist += lon_dist_interval) { + const auto prev_forward_point = + motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + if (!prev_forward_point) { + continue; + } + + // calculate lateral offset of current trajectory points to prev forward point + const auto forward_seg_idx = + motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { + return true; + } + } + + return false; +} + bool ReplanChecker::isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const { 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 new file mode 100644 index 0000000000000..75b61f80622d1 --- /dev/null +++ b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp @@ -0,0 +1,65 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "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 + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + + auto test_manager = std::make_shared(); + + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto obstacle_avoidance_planner_dir = + ament_index_cpp::get_package_share_directory("obstacle_avoidance_planner"); + + node_options.arguments( + {"--ros-args", "--params-file", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", + obstacle_avoidance_planner_dir + "/config/obstacle_avoidance_planner.param.yaml"}); + + auto test_target_node = + std::make_shared(node_options); + + // publish necessary topics from test_manager + test_manager->publishOdometry(test_target_node, "obstacle_avoidance_planner/input/odometry"); + + // set subscriber with topic name: obstacle_avoidance_planner → test_node_ + test_manager->setTrajectorySubscriber("obstacle_avoidance_planner/output/path"); + + // set obstacle_avoidance_planner's input topic name(this topic is changed to test node) + test_manager->setPathInputTopicName("obstacle_avoidance_planner/input/path"); + + // test with normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); + + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with trajectory with empty/one point/overlapping point + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 6fba830d57128..88b85ee9b8376 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -7,7 +7,9 @@ The `obstacle_cruise_planner` package has following modules. - Stop planning - stop when there is a static obstacle near the trajectory. - Cruise planning - - slow down when there is a dynamic obstacle to cruise near the trajectory + - cruise a dynamic obstacle in front of the ego. +- Slow down planning + - slow down when there is a static/dynamic obstacle near the trajectory. ## Interfaces @@ -32,9 +34,10 @@ The `obstacle_cruise_planner` package has following modules. Design for the following functions is defined here. -- Obstacle candidates selection -- Obstacle stop planning -- Adaptive cruise planning +- Behavior determination against obstacles +- Stop planning +- Cruise planning +- Slow down planning A data structure for cruise and stop planning is as follows. This planner data is created first, and then sent to the planning algorithm. @@ -66,84 +69,101 @@ struct Obstacle }; ``` -### Obstacle candidates selection +### Behavior determination against obstacles -In this function, target obstacles for stopping or cruising are selected based on their pose and velocity. +Obstacles for cruising, stopping and slowing down are selected in this order based on their pose and velocity. +The obstacles not in front of the ego will be ignored. -By default, objects that realize one of the following conditions are considered to be the target obstacle candidates. -Some terms will be defined in the following subsections. +![determine_cruise_stop_slow_down](./media/determine_cruise_stop_slow_down.drawio.svg) -- Vehicle objects "inside the detection area" other than "far crossing vehicles". -- non vehicle objects "inside the detection area" -- "Near cut-in vehicles" outside the detection area +#### Determine cruise vehicles -Note that currently the obstacle candidates selection algorithm is for autonomous driving. -However, we have following parameters as well for stop and cruise respectively so that we can extend the obstacles candidates selection algorithm for non vehicle robots. -By default, unknown and vehicles are obstacles to cruise and stop, and non vehicles are obstacles just to stop. +The obstacles meeting the following condition are determined as obstacles for cruising. -| Parameter | Type | Description | -| ------------------------------ | ---- | ------------------------------------------------- | -| `cruise_obstacle_type.unknown` | bool | flag to consider unknown objects as being cruised | -| `cruise_obstacle_type.car` | bool | flag to consider unknown objects as being cruised | -| `cruise_obstacle_type.truck` | bool | flag to consider unknown objects as being cruised | -| ... | bool | ... | -| `stop_obstacle_type.unknown` | bool | flag to consider unknown objects as being stopped | -| ... | bool | ... | +- The lateral distance from the object to the ego's trajectory is smaller than `behavior_determination.cruise.max_lat_margin`. -#### Inside the detection area +- The object type is for cruising according to `common.cruise_obstacle_type.*`. +- The object is not crossing the ego's trajectory (\*1). +- If the object is inside the trajectory. + - The object type is for inside cruising according to `common.cruise_obstacle_type.inside.*`. + - The object velocity is larger than `behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop`. +- If the object is outside the trajectory. + - The object type is for outside cruising according to `common.cruise_obstacle_type.outside.*`. + - The object velocity is larger than `behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold`. + - The highest confident predicted path collides with the ego's trajectory. + - Its collision's period is larger than `behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold`. -To calculate obstacles inside the detection area, firstly, obstacles whose distance to the trajectory is less than `rough_max_lat_margin` are selected. -Then, the detection area, which is a trajectory with some lateral margin, is calculated as shown in the figure. -The detection area width is a vehicle's width + `max_lat_margin`, and it is represented as a polygon resampled with `decimate_trajectory_step_length` longitudinally. -The roughly selected obstacles inside the detection area are considered as inside the detection area. +| Parameter | Type | Description | +| ------------------------------------------------------------------------------------ | ------ | -------------------------------------------------------------------- | +| `common.cruise_obstacle_type.inside.unknown` | bool | flag to consider unknown objects for cruising | +| `common.cruise_obstacle_type.inside.car` | bool | flag to consider unknown objects for cruising | +| `common.cruise_obstacle_type.inside.truck` | bool | flag to consider unknown objects for cruising | +| ... | bool | ... | +| `common.cruise_obstacle_type.outside.unknown` | bool | flag to consider unknown objects for cruising | +| `common.cruise_obstacle_type.outside.car` | bool | flag to consider unknown objects for cruising | +| `common.cruise_obstacle_type.outside.truck` | bool | flag to consider unknown objects for cruising | +| ... | bool | ... | +| `behavior_determination.cruise.max_lat_margin` | double | maximum lateral margin for cruise obstacles | +| `behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop` | double | maximum obstacle velocity for cruise obstacle inside the trajectory | +| `behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold` | double | maximum obstacle velocity for cruise obstacle outside the trajectory | +| `behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold` | double | maximum overlap time of the collision between the ego and obstacle | -![detection_area](./image/detection_area.png) +#### Determine stop vehicles -This two-step detection is used for calculation efficiency since collision checking of polygons is heavy. -Boost.Geometry is used as a library to check collision among polygons. +Among obstacles which are not for cruising, the obstacles meeting the following condition are determined as obstacles for stopping. -In the `behavior_determination` namespace, +- The object type is for stopping according to `common.stop_obstacle_type.*`. +- The lateral distance from the object to the ego's trajectory is smaller than `behavior_determination.stop.max_lat_margin`. +- The object velocity along the ego's trajectory is smaller than `behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise`. +- The object + - does not cross the ego's trajectory (\*1) + - with the velocity smaller than `behavior_determination.crossing_obstacle.obstacle_velocity_threshold` + - and its collision time margin is large enough (\*2). -| Parameter | Type | Description | -| --------------------------------- | ------ | ----------------------------------------------------------------------------------- | -| `rough_max_lat_margin` | double | rough lateral margin for rough detection area expansion [m] | -| `max_lat_margin` | double | lateral margin for precise detection area expansion [m] | -| `decimate_trajectory_step_length` | double | longitudinal step length to calculate trajectory polygon for collision checking [m] | +| Parameter | Type | Description | +| ------------------------------------------------------------------------ | ------ | --------------------------------------------- | +| `common.stop_obstacle_type.unknown` | bool | flag to consider unknown objects for stopping | +| `common.stop_obstacle_type.car` | bool | flag to consider unknown objects for stopping | +| `common.stop_obstacle_type.truck` | bool | flag to consider unknown objects for stopping | +| ... | bool | ... | +| `behavior_determination.stop.max_lat_margin` | double | maximum lateral margin for stop obstacles | +| `behavior_determination.crossing_obstacle.obstacle_velocity_threshold` | double | maximum crossing obstacle velocity to ignore | +| `behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise` | double | maximum obstacle velocity for stop | -#### Far crossing vehicles +#### Determine slow down vehicles -Near crossing vehicles (= not far crossing vehicles) are defined as vehicle objects realizing either of following conditions. +Among obstacles which are not for cruising and stopping, the obstacles meeting the following condition are determined as obstacles for slowing down. -- whose yaw angle against the nearest trajectory point is greater than `crossing_obstacle_traj_angle_threshold` -- whose velocity is less than `crossing_obstacle_velocity_threshold`. +- The object type is for slowing down according to `common.slow_down_obstacle_type.*`. +- The lateral distance from the object to the ego's trajectory is smaller than `behavior_determination.slow_down.max_lat_margin`. -Assuming `t_1` to be the time for the ego to reach the current crossing obstacle position with the constant velocity motion, and `t_2` to be the time for the crossing obstacle to go outside the detection area, if the following condition is realized, the crossing vehicle will be ignored. +| Parameter | Type | Description | +| ------------------------------------------------- | ------ | ------------------------------------------------- | +| `common.slow_down_obstacle_type.unknown` | bool | flag to consider unknown objects for slowing down | +| `common.slow_down_obstacle_type.car` | bool | flag to consider unknown objects for slowing down | +| `common.slow_down_obstacle_type.truck` | bool | flag to consider unknown objects for slowing down | +| ... | bool | ... | +| `behavior_determination.slow_down.max_lat_margin` | double | maximum lateral margin for slow down obstacles | -$$ -t_1 - t_2 > \mathrm{margin\_for\_collision\_time} -$$ +#### NOTE -In the `behavior_determination` namespace, +##### \*1: Crossing obstacles -| Parameter | Type | Description | -| ---------------------------------------- | ------ | ----------------------------------------------------------------------------- | -| `crossing_obstacle_velocity_threshold` | double | velocity threshold to decide crossing obstacle [m/s] | -| `crossing_obstacle_traj_angle_threshold` | double | yaw threshold of crossing obstacle against the nearest trajectory point [rad] | -| `collision_time_margin` | double | time threshold of collision between obstacle and ego [s] | +Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold`. -#### Near Cut-in vehicles +| Parameter | Type | Description | +| ------------------------------------------------------------------------ | ------ | ------------------------------------------------------------------------------------------------- | +| `behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold` | double | maximum angle against the ego's trajectory to judge the obstacle is crossing the trajectory [rad] | -Near Cut-in vehicles are defined as vehicle objects +##### \*2: Enough collision time margin -- whose predicted path's footprints from the current time to `max_prediction_time_for_collision_check` overlap with the detection area longer than `ego_obstacle_overlap_time_threshold`. +We predict the collision area and its time by the ego with a constant velocity motion and the obstacle with its predicted path. +Then, we calculate a collision time margin which is the difference of the time when the ego will be inside the collision area and the obstacle will be inside the collision area. +When this time margin is smaller than `behavior_determination.stop.crossing_obstacle.collision_time_margin`, the margin is not enough. -In the `behavior_determination` namespace, - -| Parameter | Type | Description | -| ----------------------------------------- | ------ | ------------------------------------------------------------------------ | -| `ego_obstacle_overlap_time_threshold` | double | time threshold to decide cut-in obstacle for cruise or stop [s] | -| `max_prediction_time_for_collision_check` | double | prediction time to check collision between obstacle and ego [s] | -| `outside_obstacle_min_velocity_threshold` | double | minimum velocity threshold of target obstacle for cut-in detection [m/s] | +| Parameter | Type | Description | +| --------------------------------------------------------------------- | ------ | ----------------------------------------------------- | +| `behavior_determination.stop.crossing_obstacle.collision_time_margin` | double | maximum collision time margin of the ego and obstacle | ### Stop planning @@ -155,20 +175,20 @@ In the `behavior_determination` namespace, The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects. -The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles inside the detection area. +The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles. The safe distance is parameterized as `common.safe_distance_margin`. When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes `terminal_safe_distance_margin`. When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated. If the acceleration is less than `common.min_strong_accel`, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency. -### Adaptive cruise planning +### Cruise planning | Parameter | Type | Description | | ----------------------------- | ------ | ---------------------------------------------- | | `common.safe_distance_margin` | double | minimum distance with obstacles for cruise [m] | -The role of the adaptive cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition. +The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition. This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle. The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation. @@ -189,12 +209,14 @@ These values are parameterized as follows. Other common values such as ego's min The detailed formulation is as follows. $$ -d_{error} = d - d_{rss} \\ -d_{normalized} = lpf(d_{error} / d_{obstacle}) \\ -d_{quad, normalized} = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ -v_{pid} = pid(d_{quad, normalized}) \\ -v_{add} = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ -v_{target} = max(v_{ego} + v_{add}, v_{min, cruise}) +\begin{align} +d_{error} & = d - d_{rss} \\ +d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\ +d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ +v_{pid} & = pid(d_{quad, normalized}) \\ +v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ +v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise}) +\end{align} $$ | Variable | Description | @@ -206,6 +228,35 @@ $$ | `lpf(val)` | apply low-pass filter to `val` | | `pid(val)` | apply pid to `val` | +### Slow down planning + +| Parameter | Type | Description | +| ---------------------------- | ------ | ------------------------------------------------------------------- | +| `slow_down.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m] | +| `slow_down.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m] | +| `slow_down.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m] | +| `slow_down.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m] | + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. + +The closest point on the obstacle to the ego's trajectory is calculated. +Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. + +![slow_down_velocity_calculation](./media/slow_down_velocity_calculation.svg) + +| Variable | Description | +| ---------- | ------------------------------------------------- | +| `v_{out}` | calculated velocity for slow down | +| `v_{min}` | `slow_down.min_lat_velocity` | +| `v_{max}` | `slow_down.max_lat_velocity` | +| `l_{min}` | `slow_down.min_lat_margin` | +| `l_{max}` | `slow_down.max_lat_margin` | +| `l'_{max}` | `behavior_determination.slow_down.max_lat_margin` | + +The calculated velocity is inserted in the trajectory where the obstacle is inside the area with `behavior_determination.slow_down.max_lat_margin`. + +![slow_down_planning](./media/slow_down_planning.drawio.svg) + ## Implementation ### Flowchart @@ -243,7 +294,7 @@ end group :generateCruiseTrajectory; -:getSlowDownVelocityLimit; +:generateSlowDownTrajectory; :publish trajectory; @@ -255,7 +306,7 @@ stop @enduml ``` -### Algorithm selection +### Algorithm selection for cruise planner Currently, only a PID-based planner is supported. Each planner will be explained in the following. @@ -279,7 +330,7 @@ It is the obstacle among obstacle candidates whose velocity is less than `obstac Note that, as explained in the stop planning design, a stop planning which requires a strong acceleration (less than `common.min_strong_accel`) will be canceled. -#### Adaptive cruise planning +#### Cruise planning In the `pid_based_planner` namespace, diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 63a5157ca1bc7..e7a9a7fb27c1c 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -86,14 +86,15 @@ collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: - max_lat_margin: 0.5 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego slow_down: - max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width + lat_hysteresis_margin: 0.2 cruise: pid_based_planner: @@ -166,5 +167,8 @@ min_ego_velocity: 2.0 max_ego_velocity: 8.0 - # max deceleration during slow down - max_deceleration: -1.0 + time_margin_on_target_velocity: 1.5 # [s] + + lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity + lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path + lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start diff --git a/planning/obstacle_cruise_planner/docs/debug.md b/planning/obstacle_cruise_planner/docs/debug.md index f1663b96c3626..b5487b201492e 100644 --- a/planning/obstacle_cruise_planner/docs/debug.md +++ b/planning/obstacle_cruise_planner/docs/debug.md @@ -5,41 +5,42 @@ ### Detection area Green polygons which is a detection area is visualized by `detection_polygons` in the `~/debug/marker` topic. +To determine each behavior (cruise, stop, and slow down), if `behavior_determination.*.max_lat_margin` is not zero, the polygons are expanded with the additional width. -![detection_area](../image/detection_area.png) +![detection_area](../media/detection_area.png) -### Collision point +### Collision points -Red point which is a collision point with obstacle is visualized by `collision_points` in the `~/debug/marker` topic. +Red points which are collision points with obstacle are visualized by `*_collision_points` for each behavior in the `~/debug/marker` topic. -![collision_point](../image/collision_point.png) +![collision_point](../media/collision_point.png) ### Obstacle for cruise -Yellow sphere which is a obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic. +Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic. -![obstacle_to_cruise](../image/obstacle_to_cruise.png) +Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic. -### Obstacle for stop +![cruise_visualization](../media/cruise_visualization.png) -Red sphere which is a obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic. +### Obstacle for stop -![obstacle_to_stop](../image/obstacle_to_stop.png) +Red sphere which is an obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic. - +Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/virtual_wall` topic. - +![stop_visualization](../media/stop_visualization.png) - +### Obstacle for slow down -### Obstacle cruise wall +Yellow sphere which is an obstacle for slow_down is visualized by `obstacles_to_slow_down` in the `~/debug/marker` topic. -Yellow wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise_wall_marker` topic. +Yellow wall which means a safe distance to slow_down if the ego's front meets the wall is visualized in the `~/debug/slow_down/virtual_wall` topic. -![obstacle_to_cruise](../image/obstacle_to_cruise.png) +![slow_down_visualization](../media/slow_down_visualization.png) -### Obstacle stop wall + -Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/debug/stop_wall_marker` topic. + -![obstacle_to_stop](../image/obstacle_to_stop.png) + diff --git a/planning/obstacle_cruise_planner/image/obstacle_to_cruise.png b/planning/obstacle_cruise_planner/image/obstacle_to_cruise.png deleted file mode 100644 index c5f56f59a6d7b..0000000000000 Binary files a/planning/obstacle_cruise_planner/image/obstacle_to_cruise.png and /dev/null differ diff --git a/planning/obstacle_cruise_planner/image/obstacle_to_stop.png b/planning/obstacle_cruise_planner/image/obstacle_to_stop.png deleted file mode 100644 index aee61797a6d6c..0000000000000 Binary files a/planning/obstacle_cruise_planner/image/obstacle_to_stop.png and /dev/null differ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 26018c3108e69..f6d04344e4d4b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -118,6 +118,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_slow_down_wall_marker_pub_; rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_slow_down_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber @@ -184,6 +185,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double max_lat_margin_for_stop; double max_lat_margin_for_cruise; double max_lat_margin_for_slow_down; + double lat_hysteresis_margin_for_slow_down; }; BehaviorDeterminationParam behavior_determination_param_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 1bed4030ce3e4..31707fcc9f561 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -24,6 +24,9 @@ #include #include +#include +#include +#include #include class PlannerInterface @@ -86,6 +89,11 @@ class PlannerInterface { return Float32MultiArrayStamped{}; } + Float32MultiArrayStamped getSlowDownPlanningDebugMessage(const rclcpp::Time & current_time) + { + slow_down_debug_multi_array_.stamp = current_time; + return slow_down_debug_multi_array_; + } protected: // Parameters @@ -113,6 +121,7 @@ class PlannerInterface // debug info StopPlanningDebugInfo stop_planning_debug_info_; + Float32MultiArrayStamped slow_down_debug_multi_array_; double calcDistanceToCollisionPoint( const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point); @@ -155,10 +164,35 @@ class PlannerInterface } private: - double calculateSlowDownVelocity(const SlowDownObstacle & obstacle) const; - double calculateDistanceToSlowDownWithAccConstraint( + struct SlowDownOutput + { + SlowDownOutput() = default; + SlowDownOutput( + const std::string & arg_uuid, const std::vector & traj_points, + const std::optional & start_idx, const std::optional & end_idx, + const double arg_target_vel, const double arg_precise_lat_dist) + : uuid(arg_uuid), target_vel(arg_target_vel), precise_lat_dist(arg_precise_lat_dist) + { + if (start_idx) { + start_point = traj_points.at(*start_idx).pose; + } + if (end_idx) { + end_point = traj_points.at(*end_idx).pose; + } + } + + std::string uuid; + double target_vel; + double precise_lat_dist; + std::optional start_point{std::nullopt}; + std::optional end_point{std::nullopt}; + }; + double calculateSlowDownVelocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output) const; + std::tuple calculateDistanceToSlowDownWithConstraints( const PlannerData & planner_data, const std::vector & traj_points, - const SlowDownObstacle & obstacle, const double dist_to_ego, const double slow_down_vel) const; + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const double dist_to_ego) const; struct SlowDownInfo { @@ -175,7 +209,12 @@ class PlannerInterface min_lat_margin = node.declare_parameter("slow_down.min_lat_margin"); max_ego_velocity = node.declare_parameter("slow_down.max_ego_velocity"); min_ego_velocity = node.declare_parameter("slow_down.min_ego_velocity"); - max_deceleration = node.declare_parameter("slow_down.max_deceleration"); + time_margin_on_target_velocity = + node.declare_parameter("slow_down.time_margin_on_target_velocity"); + lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); + lpf_gain_lat_dist = node.declare_parameter("slow_down.lpf_gain_lat_dist"); + lpf_gain_dist_to_slow_down = + node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); } void onParam(const std::vector & parameters) @@ -189,16 +228,27 @@ class PlannerInterface tier4_autoware_utils::updateParam( parameters, "slow_down.min_ego_velocity", min_ego_velocity); tier4_autoware_utils::updateParam( - parameters, "slow_down.max_deceleration", max_deceleration); + parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel); + tier4_autoware_utils::updateParam( + parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist); + tier4_autoware_utils::updateParam( + parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } double max_lat_margin; double min_lat_margin; double max_ego_velocity; double min_ego_velocity; - double max_deceleration; + double time_margin_on_target_velocity; + double lpf_gain_slow_down_vel; + double lpf_gain_lat_dist; + double lpf_gain_dist_to_slow_down; }; SlowDownParam slow_down_param_; + + std::vector prev_slow_down_output_; }; #endif // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/obstacle_cruise_planner/image/collision_point.png b/planning/obstacle_cruise_planner/media/collision_point.png similarity index 100% rename from planning/obstacle_cruise_planner/image/collision_point.png rename to planning/obstacle_cruise_planner/media/collision_point.png diff --git a/planning/obstacle_cruise_planner/media/cruise_visualization.png b/planning/obstacle_cruise_planner/media/cruise_visualization.png new file mode 100644 index 0000000000000..acb094135e884 Binary files /dev/null and b/planning/obstacle_cruise_planner/media/cruise_visualization.png differ diff --git a/planning/obstacle_cruise_planner/image/detection_area.png b/planning/obstacle_cruise_planner/media/detection_area.png similarity index 100% rename from planning/obstacle_cruise_planner/image/detection_area.png rename to planning/obstacle_cruise_planner/media/detection_area.png diff --git a/planning/obstacle_cruise_planner/media/determine_cruise_stop_slow_down.drawio.svg b/planning/obstacle_cruise_planner/media/determine_cruise_stop_slow_down.drawio.svg new file mode 100644 index 0000000000000..de0fba72a6ed8 --- /dev/null +++ b/planning/obstacle_cruise_planner/media/determine_cruise_stop_slow_down.drawio.svg @@ -0,0 +1,805 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + + + + + + + +
+
+
+ 1. Determine cruise vehicles +
+
+
+ + 1. Determine cruise vehicles + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + + + + + + + +
+
+
+ 2. Determine stop vehicles +
+
+
+ + 2. Determine stop vehicles + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + + + + + + + +
+
+
+ 3. Determine slow down obstacles +
+
+
+ + 3. Determine slow down obstacles + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + + + + + +
+
+
+ Plan cruise, stop, and slow down +
+
+
+ + Plan cruise, stop, and slow down + + + + + + + + + +
+
+
+ slow down +
+ obstacle +
+
+
+ + slow down... + + + + + + +
+
+
+ stop +
+ obstacle +
+
+
+ + stop... + + + + + + +
+
+
+ slow down +
+ obstacle +
+
+
+ + slow down... + + + + + + +
+
+
+ max_cruise_lat_margin +
+
+
+ + max_cruise_lat_margin + + + + + + + + + + + + +
+
+
+ max_stop_lat_margin +
+
+
+ + max_stop_lat_margin + + + + + + +
+
+
+ max_slow_down_lat_margin +
+
+
+ + max_slow_down_lat_margin + + + + + + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ parked vehicle +
+
+
+ + parked vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + +
+
+
+ running vehicle +
+
+
+ + running vehicle + + + + + + + + + + + + +
+
+
+ Scene +
+
+
+ + Scene + + + + + + + + +
+
+
+ ※ Within the margin, but not meet other cruise condition. +
+
+
+ + ※ Within the margin, but not... + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_cruise_planner/media/slow_down_planning.drawio.svg b/planning/obstacle_cruise_planner/media/slow_down_planning.drawio.svg new file mode 100644 index 0000000000000..603e6b37a653f --- /dev/null +++ b/planning/obstacle_cruise_planner/media/slow_down_planning.drawio.svg @@ -0,0 +1,133 @@ + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + +
+
+
+ trajectory velocity +
+
+
+ + trajecto... + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ max_slow_down_lat_margin +
+
+
+ + max_slow_d... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_cruise_planner/media/slow_down_velocity_calculation.svg b/planning/obstacle_cruise_planner/media/slow_down_velocity_calculation.svg new file mode 100644 index 0000000000000..ee53e6e60a0c0 --- /dev/null +++ b/planning/obstacle_cruise_planner/media/slow_down_velocity_calculation.svg @@ -0,0 +1,534 @@ + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + + + + + + + +
+
+
+ slow down +
+ velocity +
+
+
+ + slow down... + + + + + + +
+
+
+ lateral distance +
+
+
+ + lateral distance + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{max}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{min}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{min}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{max}` + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ linear interpolation of slow down velocity +
+
+
+ + linear interpolation of slow down velocity + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{out}` + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{max}... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_cruise_planner/media/slow_down_visualization.png b/planning/obstacle_cruise_planner/media/slow_down_visualization.png new file mode 100644 index 0000000000000..472b3bb6c982a Binary files /dev/null and b/planning/obstacle_cruise_planner/media/slow_down_visualization.png differ diff --git a/planning/obstacle_cruise_planner/media/stop_visualization.png b/planning/obstacle_cruise_planner/media/stop_visualization.png new file mode 100644 index 0000000000000..f3cec33fd754b Binary files /dev/null and b/planning/obstacle_cruise_planner/media/stop_visualization.png differ diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index a9c914f1bb6d5..a7c2346742137 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -13,8 +13,7 @@ Yutaka Shimizu ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_perception_msgs diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c016500eb710e..13d43fbe3b08e 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -202,6 +202,21 @@ geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & poin geom_point.y = point.y(); return geom_point; } + +bool isLowerConsideringHysteresis( + const double current_val, const bool was_low, const double high_val, const double low_val) +{ + if (was_low) { + if (high_val < current_val) { + return false; + } + return true; + } + if (current_val < low_val) { + return true; + } + return false; +} } // namespace namespace motion_planning @@ -244,6 +259,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara node.declare_parameter("behavior_determination.cruise.max_lat_margin"); max_lat_margin_for_slow_down = node.declare_parameter("behavior_determination.slow_down.max_lat_margin"); + lat_hysteresis_margin_for_slow_down = + node.declare_parameter("behavior_determination.slow_down.lat_hysteresis_margin"); } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -293,6 +310,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); tier4_autoware_utils::updateParam( parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.slow_down.lat_hysteresis_margin", + lat_hysteresis_margin_for_slow_down); } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -327,12 +347,15 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); - debug_slow_down_wall_marker_pub_ = create_publisher("~/slow_down/virtual_wall", 1); + debug_slow_down_wall_marker_pub_ = + create_publisher("~/debug/slow_down/virtual_wall", 1); debug_marker_pub_ = create_publisher("~/debug/marker", 1); debug_stop_planning_info_pub_ = create_publisher("~/debug/stop_planning_info", 1); debug_cruise_planning_info_pub_ = create_publisher("~/debug/cruise_planning_info", 1); + debug_slow_down_planning_info_pub_ = + create_publisher("~/debug/slow_down_planning_info", 1); const auto longitudinal_info = LongitudinalInfo(*this); @@ -730,8 +753,10 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( { // consider hysteresis const double obstacle_projected_vel = calcObstacleProjectedVelocity(traj_points, obstacle); - const auto is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, obstacle.uuid); - const auto is_prev_obstacle_cruise = getObstacleFromUuid(prev_cruise_obstacles_, obstacle.uuid); + // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, + // obstacle.uuid).has_value(); + const bool is_prev_obstacle_cruise = + getObstacleFromUuid(prev_cruise_obstacles_, obstacle.uuid).has_value(); if (is_prev_obstacle_cruise) { if (obstacle_projected_vel < p.obstacle_velocity_threshold_from_cruise_to_stop) { @@ -927,9 +952,23 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; - if ( - !enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label) || - p.max_lat_margin_for_slow_down < precise_lat_dist) { + const bool is_prev_obstacle_slow_down = + getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + + if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { + return std::nullopt; + } + + // check lateral distance considering hysteresis + const bool is_lat_dist_low = isLowerConsideringHysteresis( + precise_lat_dist, is_prev_obstacle_slow_down, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, + p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); + if (!is_lat_dist_low) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), + precise_lat_dist); return std::nullopt; } @@ -1136,7 +1175,6 @@ PlannerData ObstacleCruisePlannerNode::createPlannerData( planner_data.ego_vel = ego_odom_ptr_->twist.twist.linear.x; planner_data.ego_acc = ego_accel_ptr_->accel.accel.linear.x; planner_data.is_driving_forward = is_driving_forward_; - return planner_data; } @@ -1286,8 +1324,8 @@ void ObstacleCruisePlannerNode::publishDebugInfo() const debug_cruise_planning_info_pub_->publish(cruise_debug_msg); // slow_down - // const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now()); - // debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); + const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now()); + debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); } void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp index 33a742fe1ab02..c8932da6f17e5 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -14,7 +14,7 @@ #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" -#include +#include #include @@ -248,29 +248,35 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza const std::vector optval = std::get<0>(result); const int status_val = std::get<3>(result); - if (status_val != 1) { + if (status_val != 1) std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl; - } - std::vector opt_time = time_vec; - std::vector opt_pos(N); - std::vector opt_vel(N); - std::vector opt_acc(N); - std::vector opt_jerk(N); - for (size_t i = 0; i < N; ++i) { - opt_pos.at(i) = optval.at(IDX_S0 + i); - opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); - opt_acc.at(i) = optval.at(IDX_A0 + i); - opt_jerk.at(i) = optval.at(IDX_J0 + i); - } - opt_vel.back() = 0.0; + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) std::cerr << "optimization failed : result contains NaN values\n"; OptimizationResult optimized_result; - optimized_result.t = opt_time; - optimized_result.s = opt_pos; - optimized_result.v = opt_vel; - optimized_result.a = opt_acc; - optimized_result.j = opt_jerk; + const auto is_optimization_failed = status_val != 1 || has_nan; + if (!is_optimization_failed) { + std::vector opt_time = time_vec; + std::vector opt_pos(N); + std::vector opt_vel(N); + std::vector opt_acc(N); + std::vector opt_jerk(N); + for (size_t i = 0; i < N; ++i) { + opt_pos.at(i) = optval.at(IDX_S0 + i); + opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); + opt_acc.at(i) = optval.at(IDX_A0 + i); + opt_jerk.at(i) = optval.at(IDX_J0 + i); + } + opt_vel.back() = 0.0; + + optimized_result.t = opt_time; + optimized_result.s = opt_pos; + optimized_result.v = opt_vel; + optimized_result.a = opt_acc; + optimized_result.j = opt_jerk; + } return optimized_result; } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 44b8ea4584a0a..2786b6dae9f6c 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -14,6 +14,9 @@ #include "obstacle_cruise_planner/planner_interface.hpp" +#include "motion_utils/distance/distance.hpp" +#include "signal_processing/lowpass_filter_1d.hpp" + namespace { StopSpeedExceeded createStopSpeedExceededMsg( @@ -100,6 +103,107 @@ double calcMinimumDistanceToStop( return -std::pow(initial_vel, 2) / 2.0 / min_acc; } + +template +std::optional getObjectFromUuid(const std::vector & objects, const std::string & target_uuid) +{ + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; +} + +// TODO(murooka) following two functions are copied from behavior_velocity_planner. +// These should be refactored. +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max) +{ + const double j = jerk; + const double a = accel; + const double v = velocity; + const double d = distance; + const double min = t_min; + const double max = t_max; + auto f = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { + std::logic_error("[obstacle_cruise_planner](findReachTime): search range is invalid"); + } + const double eps = 1e-5; + const int warn_iter = 100; + double lower = min; + double upper = max; + double t; + int iter = 0; + for (int i = 0;; i++) { + t = 0.5 * (lower + upper); + const double fx = f(t, j, a, v, d); + // std::cout<<"fx: "< 0.0) { + upper = t; + } else { + lower = t; + } + iter++; + if (iter > warn_iter) + std::cerr << "[obstacle_cruise_planner](findReachTime): current iter is over warning" + << std::endl; + } + return t; +} + +double calcDecelerationVelocityFromDistanceToTarget( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target) +{ + if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) { + std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + } + // case0: distance to target is behind ego + if (distance_to_target <= 0) return current_velocity; + auto ft = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + auto vt = [](const double t, const double j, const double a, const double v) { + return j * t * t / 2.0 + a * t + v; + }; + const double j_max = max_slowdown_jerk; + const double a0 = current_accel; + const double a_max = max_slowdown_accel; + const double v0 = current_velocity; + const double l = distance_to_target; + const double t_const_jerk = (a_max - a0) / j_max; + const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); + const double d_const_acc_stop = l - d_const_jerk_stop; + + if (d_const_acc_stop < 0) { + // case0: distance to target is within constant jerk deceleration + // use binary search instead of solving cubic equation + const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk); + const double velocity = vt(t_jerk, j_max, a0, v0); + return velocity; + } else { + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); + } + return current_velocity; +} } // namespace std::vector PlannerInterface::generateStopTrajectory( @@ -282,6 +386,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( { stop_watch_.tic(__func__); auto slow_down_traj_points = cruise_traj_points; + slow_down_debug_multi_array_ = Float32MultiArrayStamped(); const double dist_to_ego = [&]() { const size_t ego_seg_idx = @@ -294,11 +399,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); // define function to insert slow down velocity to trajectory - const auto insert_slow_down_to_trajectory = - [&](const double lon_dist, const double slow_down_vel) -> std::optional { + const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional { const auto inserted_idx = motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); if (inserted_idx) { - slow_down_traj_points.at(inserted_idx.get()).longitudinal_velocity_mps = slow_down_vel; + if (inserted_idx.get() + 1 <= slow_down_traj_points.size() - 1) { + // zero-order hold for velocity interpolation + slow_down_traj_points.at(inserted_idx.get()).longitudinal_velocity_mps = + slow_down_traj_points.at(inserted_idx.get() + 1).longitudinal_velocity_mps; + } return inserted_idx.get(); } return std::nullopt; @@ -315,46 +423,68 @@ std::vector PlannerInterface::generateSlowDownTrajectory( tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); }; + std::vector new_prev_slow_down_output; for (size_t i = 0; i < obstacles.size(); ++i) { const auto & obstacle = obstacles.at(i); - - // calculate slow down velocity - const double slow_down_vel = calculateSlowDownVelocity(obstacle); + const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid); // calculate slow down start distance, and insert slow down velocity - const double dist_to_slow_down_start = calculateDistanceToSlowDownWithAccConstraint( - planner_data, slow_down_traj_points, obstacle, dist_to_ego, slow_down_vel); - const auto slow_down_start_idx = - insert_slow_down_to_trajectory(dist_to_slow_down_start, slow_down_vel); - if (!slow_down_start_idx) { - continue; - } + const auto [dist_to_slow_down_start, dist_to_slow_down_end, feasible_slow_down_vel] = + calculateDistanceToSlowDownWithConstraints( + planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego); // calculate slow down end distance, and insert slow down velocity - const double dist_to_slow_down_end = - motion_utils::calcSignedArcLength(slow_down_traj_points, 0, obstacle.back_collision_point) - - abs_ego_offset; // NOTE: slow_down_start_idx will not be wrong since inserted back point is after inserted // front point. - const auto slow_down_end_idx = - dist_to_slow_down_start < dist_to_slow_down_end - ? insert_slow_down_to_trajectory(dist_to_slow_down_end, slow_down_vel) - : std::nullopt; + const auto slow_down_start_idx = insert_point_in_trajectory(dist_to_slow_down_start); + const auto slow_down_end_idx = dist_to_slow_down_start < dist_to_slow_down_end + ? insert_point_in_trajectory(dist_to_slow_down_end) + : std::nullopt; + if (!slow_down_end_idx) { + continue; + } + + // calculate slow down velocity + const double stable_slow_down_vel = [&]() { + if (prev_output) { + return signal_processing::lowpassFilter( + feasible_slow_down_vel, prev_output->target_vel, slow_down_param_.lpf_gain_slow_down_vel); + } + return feasible_slow_down_vel; + }(); // insert slow down velocity between slow start and end + for (size_t i = (slow_down_start_idx ? *slow_down_start_idx : 0); i <= *slow_down_end_idx; + ++i) { + auto & traj_point = slow_down_traj_points.at(i); + traj_point.longitudinal_velocity_mps = + std::min(traj_point.longitudinal_velocity_mps, static_cast(stable_slow_down_vel)); + } + + // add debug data and virtual wall + slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist); + slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); + if (slow_down_start_idx) { + slow_down_debug_multi_array_.data.push_back( + slow_down_start_idx ? *slow_down_start_idx : -1.0); + add_slow_down_marker(i, slow_down_start_idx, true); + } if (slow_down_end_idx) { - for (size_t i = *slow_down_start_idx; i <= *slow_down_end_idx; ++i) { - slow_down_traj_points.at(i).longitudinal_velocity_mps = slow_down_vel; - } + slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0); + add_slow_down_marker(i, slow_down_end_idx, false); } debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); - // virtual wall marker for stop obstacle - add_slow_down_marker(i, slow_down_start_idx, true); - add_slow_down_marker(i, slow_down_end_idx, false); + // update prev_slow_down_output_ + new_prev_slow_down_output.push_back(SlowDownOutput{ + obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx, + stable_slow_down_vel, obstacle.precise_lat_dist}); } + // update prev_slow_down_output_ + prev_slow_down_output_ = new_prev_slow_down_output; + const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::SlowDownPlanner"), enable_calculation_time_info_, @@ -363,13 +493,22 @@ std::vector PlannerInterface::generateSlowDownTrajectory( return slow_down_traj_points; } -double PlannerInterface::calculateSlowDownVelocity(const SlowDownObstacle & obstacle) const +double PlannerInterface::calculateSlowDownVelocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output) const { const auto & p = slow_down_param_; + const double stable_precise_lat_dist = [&]() { + if (prev_output) { + return signal_processing::lowpassFilter( + obstacle.precise_lat_dist, prev_output->precise_lat_dist, + slow_down_param_.lpf_gain_lat_dist); + } + return obstacle.precise_lat_dist; + }(); + const double ratio = std::clamp( - (std::abs(obstacle.precise_lat_dist) - p.min_lat_margin) / - (p.max_lat_margin - p.min_lat_margin), + (std::abs(stable_precise_lat_dist) - p.min_lat_margin) / (p.max_lat_margin - p.min_lat_margin), 0.0, 1.0); const double slow_down_vel = p.min_ego_velocity + ratio * (p.max_ego_velocity - p.min_ego_velocity); @@ -377,37 +516,79 @@ double PlannerInterface::calculateSlowDownVelocity(const SlowDownObstacle & obst return slow_down_vel; } -double PlannerInterface::calculateDistanceToSlowDownWithAccConstraint( +std::tuple PlannerInterface::calculateDistanceToSlowDownWithConstraints( const PlannerData & planner_data, const std::vector & traj_points, - const SlowDownObstacle & obstacle, const double dist_to_ego, const double slow_down_vel) const + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const double dist_to_ego) const { const auto & p = slow_down_param_; const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); - // calculate distance between start of path and slow down point - const double dist_to_slow_down = - motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point) - - abs_ego_offset; - if (std::abs(planner_data.ego_vel) < std::abs(slow_down_vel)) { + // calculate slow down velocity + const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output); + + // calculate distance to collision points + const double dist_to_front_collision = + motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); + const double dist_to_back_collision = + motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); + + // calculate distance during deceleration, slow down preparation, and slow down + const double slow_down_prepare_dist = slow_down_vel * p.time_margin_on_target_velocity; + const double deceleration_dist = + dist_to_front_collision - abs_ego_offset - dist_to_ego - slow_down_prepare_dist; + const double slow_down_dist = + dist_to_back_collision - dist_to_front_collision + slow_down_prepare_dist; + + // calculate distance to start/end slow down + const double dist_to_slow_down_start = dist_to_ego + deceleration_dist; + const double dist_to_slow_down_end = dist_to_ego + deceleration_dist + slow_down_dist; + + // apply low-pass filter to distance to start/end slow down + const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) { + if (prev_output && prev_point) { + const size_t seg_idx = + motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + const double prev_dist_to_slow_down = + motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); + return signal_processing::lowpassFilter( + dist_to_slow_down, prev_dist_to_slow_down, slow_down_param_.lpf_gain_dist_to_slow_down); + } return dist_to_slow_down; - } - - // calculate deceleration - const double dist_to_slow_down_from_ego = dist_to_slow_down - dist_to_ego; - const double limited_slow_down_acc = [&]() { - if (dist_to_slow_down_from_ego <= 0.0) { - return p.max_deceleration; + }; + const double filtered_dist_to_slow_down_start = + apply_lowpass_filter(dist_to_slow_down_start, prev_output->start_point); + const double filtered_dist_to_slow_down_end = + apply_lowpass_filter(dist_to_slow_down_end, prev_output->end_point); + + // calculate velocity considering constraints + const double feasible_slow_down_vel = [&]() { + if (deceleration_dist < 0) { + if (prev_output) { + return prev_output->target_vel; + } + return slow_down_vel; + } + if (planner_data.ego_vel < slow_down_vel) { + return slow_down_vel; } - const double slow_down_acc = -(std::pow(planner_data.ego_vel, 2) - std::pow(slow_down_vel, 2)) / - 2.0 / dist_to_slow_down_from_ego; - return std::max(slow_down_acc, p.max_deceleration); + if (planner_data.ego_acc < longitudinal_info_.min_accel) { + const double squared_vel = + std::pow(planner_data.ego_vel, 2) + 2 * deceleration_dist * longitudinal_info_.min_accel; + if (squared_vel < 0) { + return slow_down_vel; + } + return std::max(std::sqrt(squared_vel), slow_down_vel); + } + // TODO(murooka) Calculate more precisely. Final acceleration should be zero. + const double min_feasible_slow_down_vel = calcDecelerationVelocityFromDistanceToTarget( + longitudinal_info_.min_jerk, longitudinal_info_.min_accel, planner_data.ego_acc, + planner_data.ego_vel, deceleration_dist); + return std::max(min_feasible_slow_down_vel, slow_down_vel); }(); - // calculate lon_dist backwards from limited decleration - const double limited_dist_to_slow_down_from_ego = - -(std::pow(planner_data.ego_vel, 2) - std::pow(slow_down_vel, 2)) / 2.0 / limited_slow_down_acc; - - return limited_dist_to_slow_down_from_ego + dist_to_ego; + return std::make_tuple( + filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); } 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 266a8553295c5..5be04ab801e14 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,20 +21,31 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +using motion_planning::ObstacleCruisePlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() { - rclcpp::init(0, nullptr); + auto test_manager = std::make_shared(); - auto test_manager = std::make_shared(); + // set subscriber with topic name: obstacle_cruise_planner → test_node_ + test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory"); - auto node_options = rclcpp::NodeOptions{}; + // set obstacle_cruise_planners input topic name(this topic is changed to test node): + test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory"); + + test_manager->setOdometryTopicName("obstacle_cruise_planner/input/odometry"); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; const auto obstacle_cruise_planner_dir = ament_index_cpp::get_package_share_directory("obstacle_cruise_planner"); - const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", @@ -42,25 +53,51 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) "--params-file", obstacle_cruise_planner_dir + "/config/default_common.param.yaml", "--params-file", obstacle_cruise_planner_dir + "/config/obstacle_cruise_planner.param.yaml"}); - auto test_target_node = - std::make_shared(node_options); + return std::make_shared(node_options); +} +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "obstacle_cruise_planner/input/odometry"); test_manager->publishPredictedObjects(test_target_node, "obstacle_cruise_planner/input/objects"); test_manager->publishAcceleration(test_target_node, "obstacle_cruise_planner/input/acceleration"); +} - // set subscriber with topic name: obstacle_cruise_planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // set obstacle_cruise_planners input topic name(this topic is changed to test node): - test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory"); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + + rclcpp::shutdown(); +} +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - test_manager->testWithAbnormalTrajectory(test_target_node); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } diff --git a/planning/obstacle_stop_planner/CMakeLists.txt b/planning/obstacle_stop_planner/CMakeLists.txt index a842aabd665b7..5b7edcd559fe5 100644 --- a/planning/obstacle_stop_planner/CMakeLists.txt +++ b/planning/obstacle_stop_planner/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) find_package(PCL REQUIRED) ament_auto_add_library(obstacle_stop_planner SHARED @@ -17,13 +16,11 @@ ament_auto_add_library(obstacle_stop_planner SHARED target_include_directories(obstacle_stop_planner SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) target_link_libraries(obstacle_stop_planner - ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 41c8b96f27ede..ef5b0facd236e 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -31,19 +31,26 @@ ### Common Parameter -| Parameter | Type | Description | -| ---------------------------------- | ------ | ----------------------------------------------------------------------------------------- | -| `enable_slow_down` | bool | enable slow down planner [-] | -| `max_velocity` | double | max velocity [m/s] | -| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | -| `enable_z_axis_obstacle_filtering` | bool | filter obstacles in z axis (height) [-] | -| `z_axis_filtering_buffer` | double | additional buffer for z axis filtering [m]] | +| Parameter | Type | Description | +| -------------------------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | +| `enable_z_axis_obstacle_filtering` | bool | filter obstacles in z axis (height) [-] | +| `z_axis_filtering_buffer` | double | additional buffer for z axis filtering [m] | +| `use_predicted_objects` | bool | whether to use predicted objects for collision and slowdown detection [-] | +| `predicted_object_filtering_threshold` | double | threshold for filtering predicted objects [valid only publish_obstacle_polygon true] [m] | +| `publish_obstacle_polygon` | bool | if use_predicted_objects is true, node publishes collision polygon [-] | ## Obstacle Stop Planner ### Role -This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `baselink`(center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points for reducing computational costs.) +This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum +of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `baselink`( +center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as +following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points +for reducing computational costs.)
![example](./docs/collision_parameters.svg){width=1000} @@ -55,20 +62,29 @@ This module inserts the stop point before the obstacle with margin. In nominal c
target for obstacle stop planner
-If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.) +If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum +of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For +example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.)
![example](./docs/min_longitudinal_margin.svg){width=1000}
minimum longitudinal margin
-The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. +The module searches the obstacle pointcloud within detection area. When the pointcloud is +found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not +insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. ### Restart prevention -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control +performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to +moving in order to approach the near stop point (e.g. 0.3 meters away). -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module, the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle +is stopped within `hold_stop_margin_distance` meters from stop point of the module, the module judges that the vehicle +has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is +stopped due to other factors.
![example](./docs/restart_prevention.svg){width=1000} @@ -141,7 +157,10 @@ stop ### Role -This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation. +This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward +margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum +of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The +velocity is calculated the following equation. $v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$ @@ -152,7 +171,8 @@ $v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} - $l_{margin}$ : `lateral_margin` [m] - $l_{vw}$ : width of the ego footprint [m] -The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section. +The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow +down section.
![example](./docs/slow_down_parameters.svg){width=1000} @@ -218,40 +238,42 @@ stop ### Role -`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car). - -| Parameter | Type | Description | -| ---------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------- | -| `adaptive_cruise_control.use_object_to_estimate_vel` | bool | use dynamic objects for estimating object velocity or not | -| `adaptive_cruise_control.use_pcl_to_estimate_vel` | bool | use raw pointclouds for estimating object velocity or not | -| `adaptive_cruise_control.consider_obj_velocity` | bool | consider forward vehicle velocity to calculate target velocity in adaptive cruise or not | -| `adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc` | double | start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] | -| `adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc` | double | stop acc when the velocity of the forward obstacle falls below this value [m/s] | -| `adaptive_cruise_control.emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | -| `adaptive_cruise_control.emergency_stop_idling_time` | double | supposed idling time to start emergency stop [s] | -| `adaptive_cruise_control.min_dist_stop` | double | minimum distance of emergency stop [m] | -| `adaptive_cruise_control.obstacle_emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | -| `adaptive_cruise_control.max_standard_acceleration` | double | supposed maximum acceleration in active cruise control [m/ss] | -| `adaptive_cruise_control.min_standard_acceleration` | double | supposed minimum acceleration (deceleration) in active cruise control [m/ss] | -| `adaptive_cruise_control.standard_idling_time` | double | supposed idling time to react object in active cruise control [s] | -| `adaptive_cruise_control.min_dist_standard` | double | minimum distance in active cruise control [m] | -| `adaptive_cruise_control.obstacle_min_standard_acceleration` | double | supposed minimum acceleration of forward obstacle [m/ss] | -| `adaptive_cruise_control.margin_rate_to_change_vel` | double | rate of margin distance to insert target velocity [-] | -| `adaptive_cruise_control.use_time_compensation_to_calc_distance` | bool | use time-compensation to calculate distance to forward vehicle | -| `adaptive_cruise_control.p_coefficient_positive` | double | coefficient P in PID control (used when target dist -current_dist >=0) [-] | -| `adaptive_cruise_control.p_coefficient_negative` | double | coefficient P in PID control (used when target dist -current_dist <0) [-] | -| `adaptive_cruise_control.d_coefficient_positive` | double | coefficient D in PID control (used when delta_dist >=0) [-] | -| `adaptive_cruise_control.d_coefficient_negative` | double | coefficient D in PID control (used when delta_dist <0) [-] | -| `adaptive_cruise_control.object_polygon_length_margin` | double | The distance to extend the polygon length the object in pointcloud-object matching [m] | -| `adaptive_cruise_control.object_polygon_width_margin` | double | The distance to extend the polygon width the object in pointcloud-object matching [m] | -| `adaptive_cruise_control.valid_estimated_vel_diff_time` | double | Maximum time difference treated as continuous points in speed estimation using a point cloud [s] | -| `adaptive_cruise_control.valid_vel_que_time` | double | Time width of information used for speed estimation in speed estimation using a point cloud [s] | -| `adaptive_cruise_control.valid_estimated_vel_max` | double | Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] | -| `adaptive_cruise_control.valid_estimated_vel_min` | double | Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] | -| `adaptive_cruise_control.thresh_vel_to_stop` | double | Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] | -| `adaptive_cruise_control.lowpass_gain_of_upper_velocity` | double | Lowpass-gain of target velocity | -| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed | -| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | +`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the +trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of +the front car), and the distance to the point cloud (= distance to the front car). + +| Parameter | Type | Description | +| ---------------------------------------------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------- | +| `adaptive_cruise_control.use_object_to_estimate_vel` | bool | use dynamic objects for estimating object velocity or not (valid only if osp.use_predicted_objects false) | +| `adaptive_cruise_control.use_pcl_to_estimate_vel` | bool | use raw pointclouds for estimating object velocity or not (valid only if osp.use_predicted_objects false) | +| `adaptive_cruise_control.consider_obj_velocity` | bool | consider forward vehicle velocity to calculate target velocity in adaptive cruise or not | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc` | double | start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc` | double | stop acc when the velocity of the forward obstacle falls below this value [m/s] | +| `adaptive_cruise_control.emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.emergency_stop_idling_time` | double | supposed idling time to start emergency stop [s] | +| `adaptive_cruise_control.min_dist_stop` | double | minimum distance of emergency stop [m] | +| `adaptive_cruise_control.obstacle_emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.max_standard_acceleration` | double | supposed maximum acceleration in active cruise control [m/ss] | +| `adaptive_cruise_control.min_standard_acceleration` | double | supposed minimum acceleration (deceleration) in active cruise control [m/ss] | +| `adaptive_cruise_control.standard_idling_time` | double | supposed idling time to react object in active cruise control [s] | +| `adaptive_cruise_control.min_dist_standard` | double | minimum distance in active cruise control [m] | +| `adaptive_cruise_control.obstacle_min_standard_acceleration` | double | supposed minimum acceleration of forward obstacle [m/ss] | +| `adaptive_cruise_control.margin_rate_to_change_vel` | double | rate of margin distance to insert target velocity [-] | +| `adaptive_cruise_control.use_time_compensation_to_calc_distance` | bool | use time-compensation to calculate distance to forward vehicle | +| `adaptive_cruise_control.p_coefficient_positive` | double | coefficient P in PID control (used when target dist -current_dist >=0) [-] | +| `adaptive_cruise_control.p_coefficient_negative` | double | coefficient P in PID control (used when target dist -current_dist <0) [-] | +| `adaptive_cruise_control.d_coefficient_positive` | double | coefficient D in PID control (used when delta_dist >=0) [-] | +| `adaptive_cruise_control.d_coefficient_negative` | double | coefficient D in PID control (used when delta_dist <0) [-] | +| `adaptive_cruise_control.object_polygon_length_margin` | double | The distance to extend the polygon length the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.object_polygon_width_margin` | double | The distance to extend the polygon width the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.valid_estimated_vel_diff_time` | double | Maximum time difference treated as continuous points in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_vel_que_time` | double | Time width of information used for speed estimation in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_estimated_vel_max` | double | Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.valid_estimated_vel_min` | double | Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.thresh_vel_to_stop` | double | Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] | +| `adaptive_cruise_control.lowpass_gain_of_upper_velocity` | double | Lowpass-gain of target velocity | +| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed (valid only if osp.use_predicted_objects false) | +| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | ### Flowchart @@ -317,37 +339,61 @@ stop This module works only when the target point is found in the detection area of the `Obstacle stop planner` module. -The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the perception failure. -If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the target point velocity. -Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the estimation, the median of the calculation result for several steps is used. +The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses +the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. +The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the +perception failure. +If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the +dynamic object is used as the target point velocity. +Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; +that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the +target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the +estimation, the median of the calculation result for several steps is used. If the calculated velocity is within the threshold range, it is used as the target point velocity. -Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated. +Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, +the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode +transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, +and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated +distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity +to insert is calculated. The emergency distance $d\_{emergency}$ is calculated as follows. -$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$ +$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_ +{emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$ -- $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends on the parameter `min_dist_stop` -- $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the parameter `emergency_stop_idling_time` +- $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends + on the parameter `min_dist_stop` +- $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the + parameter `emergency_stop_idling_time` - $v_{ego}$ is a current velocity of own vehicle -- $a_{ego_{_{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{emergency}}}$ depends on the parameter `emergency_stop_acceleration` +- $a_{ego_{_{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_ + {emergency}}}$ depends on the parameter `emergency_stop_acceleration` - $v_{obj}$ is a current velocity of obstacle pointcloud. -- $a_{obj_{_{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{emergency}}}$ depends on the parameter `obstacle_emergency_stop_acceleration` +- $a_{obj_{_{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_ + {emergency}}}$ depends on the parameter `obstacle_emergency_stop_acceleration` - \*Above $X_{_{emergency}}$ parameters are used only in emergency situation. -The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard distance $d\_{standard}$ calculated as following. Therefore, if the distance -to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current velocity, and vice versa. For keeping the distance, a PID controller is used. +The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard +distance $d\_{standard}$ calculated as following. Therefore, if the distance +to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current +velocity, and vice versa. For keeping the distance, a PID controller is used. -$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$ +$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_ +{standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$ -- $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends on the parameter `min_dist_stop` -- $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the parameter `standard_stop_idling_time` +- $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends + on the parameter `min_dist_stop` +- $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the + parameter `standard_stop_idling_time` - $v_{ego}$ is a current velocity of own vehicle -- $a_{ego_{_{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{standard}}}$ depends on the parameter `min_standard_acceleration` +- $a_{ego_{_{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_ + {standard}}}$ depends on the parameter `min_standard_acceleration` - $v_{obj}$ is a current velocity of obstacle pointcloud. -- $a_{obj_{_{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{standard}}}$ depends on the parameter `obstacle_min_standard_acceleration` +- $a_{obj_{_{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_ + {standard}}}$ depends on the parameter `obstacle_min_standard_acceleration` - \*Above $X_{_{standard}}$ parameters are used only in non-emergency situation. ![adaptive_cruise](./docs/adaptive_cruise.drawio.svg) @@ -356,8 +402,14 @@ If the target velocity exceeds the value of `thresh_vel_to_stop`, the target vel ## Known Limits -- It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded by `Adaptive Cruise Controller` module. If the velocity planning module is updated, please take care of the vehicle's behavior as much as possible and always be ready for overriding. +- It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded + by `Adaptive Cruise Controller` module. If the velocity planning module is updated, please take care of the vehicle's + behavior as much as possible and always be ready for overriding. -- The velocity estimation algorithm in `Adaptive Cruise Controller` is depend on object tracking module. Please note that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves dangerously. +- The velocity estimation algorithm in `Adaptive Cruise Controller` is depend on object tracking module. Please note + that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves + dangerously. -- It does not work for backward driving, but publishes the path of the input as it is. Please use [obstacle_cruise_planner](../obstacle_cruise_planner/README.md) if you want to stop against an obstacle when backward driving. +- It does not work for backward driving, but publishes the path of the input as it is. Please + use [obstacle_cruise_planner](../obstacle_cruise_planner/README.md) if you want to stop against an obstacle when + backward driving. diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index a78fe5ea537d7..f97b2b57794e3 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,15 +1,16 @@ /**: ros__parameters: - chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] - max_velocity: 20.0 # max velocity [m/s] - enable_slow_down: False # whether to use slow down planner [-] - enable_z_axis_obstacle_filtering: True # filter obstacles in z axis (height) [-] - z_axis_filtering_buffer: 0.0 # additional buffer for z axis filtering [m] - voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] - voxel_grid_y: 0.05 # voxel grid y parameter for filtering pointcloud [m] - voxel_grid_z: 100000.0 # voxel grid z parameter for filtering pointcloud [m] - use_predicted_objects : False # whether to use predicted objects [-] - publish_obstacle_polygon: False # whether to publish obstacle polygon [-] + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] + max_velocity: 20.0 # max velocity [m/s] + enable_slow_down: False # whether to use slow down planner [-] + enable_z_axis_obstacle_filtering: True # filter obstacles in z axis (height) [-] + z_axis_filtering_buffer: 0.0 # additional buffer for z axis filtering [m] + voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] + voxel_grid_y: 0.05 # voxel grid y parameter for filtering pointcloud [m] + voxel_grid_z: 100000.0 # voxel grid z parameter for filtering pointcloud [m] + use_predicted_objects : False # whether to use predicted objects [-] + publish_obstacle_polygon: False # whether to publish obstacle polygon [-] + predicted_object_filtering_threshold: 1.5 # threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m] stop_planner: # params for stop position diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index b93faad3a4131..40a3f36bff903 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include +#include #include #include @@ -32,6 +33,7 @@ namespace motion_planning { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObject; class AdaptiveCruiseController { public: @@ -47,6 +49,14 @@ class AdaptiveCruiseController const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); + void insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header, + const PredictedObject & target_object); + private: rclcpp::Publisher::SharedPtr pub_debug_; @@ -187,6 +197,8 @@ class AdaptiveCruiseController bool estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, double * velocity); + void calculateProjectedVelocityFromObject( + const PredictedObject & object, const double traj_yaw, double * velocity); double estimateRoughPointVelocity(double current_vel); bool isObstacleVelocityHigh(const double obj_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index 597601cc4a5c7..23000d97ec782 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -14,9 +14,6 @@ #ifndef OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ #define OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ -#include -#include -#include #include #include @@ -33,8 +30,9 @@ #include #include #define EIGEN_MPL2_ONLY -#include -#include +#include +#include +#include namespace motion_planning { @@ -108,10 +106,10 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const std::vector & polygon, const double z, const PolygonType & type); + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const std::vector & polyhedron, const double z_min, const double z_max, + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); bool pushPose(const Pose & pose, const PoseType & type); 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 4d590d9a141ab..c836b380e5cf2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -21,9 +21,6 @@ #include #include -#include -#include -#include #include #include #include @@ -91,7 +88,7 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; - +using autoware_auto_perception_msgs::msg::PredictedObject; struct ObstacleWithDetectionTime { explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) @@ -105,13 +102,15 @@ struct ObstacleWithDetectionTime struct PredictedObjectWithDetectionTime { - explicit PredictedObjectWithDetectionTime(const rclcpp::Time & t, Pose & p) - : detection_time(t), point(p) + explicit PredictedObjectWithDetectionTime( + const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) + : detection_time(t), point(p), object(std::move(obj)) { } rclcpp::Time detection_time; - Pose point; + geometry_msgs::msg::Point point; + PredictedObject object; }; class ObstacleStopPlannerNode : public rclcpp::Node @@ -160,6 +159,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node bool set_velocity_limit_{false}; + double object_filtering_margin_{}; // only valid if use_predicted_objects is true + VehicleInfo vehicle_info_; NodeParam node_param_; StopParam stop_param_; @@ -176,8 +177,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr); void searchPredictedObject( - const TrajectoryPoints & decimate_trajectory, PlannerData & planner_data, - const VehicleInfo & vehicle_info, const StopParam & stop_param); + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param); void insertVelocity( TrajectoryPoints & trajectory, PlannerData & planner_data, const Header & trajectory_header, @@ -218,6 +220,10 @@ class ObstacleStopPlannerNode : public rclcpp::Node void publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel); + void filterObstacles( + const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj, + const double dist_threshold, PredictedObjects & filtered_objects); + // Callback void onTrigger(const Trajectory::ConstSharedPtr input_msg); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 1835c33b9a65b..6b8e2b8b3ec9d 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -86,8 +87,14 @@ struct NodeParam // voxel grid z parameter for filtering pointcloud [m] double voxel_grid_z; + // It uses only predicted objects for slowdown and collision checking bool use_predicted_objects; + // If use_predicted_objects is true, objects are ignored if distance to trajectory is larger than + // this value [m] + double predicted_object_filtering_threshold; + + // If use_predicted_objects is true, node publishes collision polygon bool publish_obstacle_polygon; }; @@ -249,11 +256,7 @@ struct PlannerData pcl::PointXYZ lateral_nearest_slow_down_point; - Pose nearest_collision_point_pose{}; - - Pose nearest_slow_down_point_pose{}; - - Pose lateral_nearest_slow_down_point_pose{}; + autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{}; rclcpp::Time nearest_collision_point_time{}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index a07fcbc537855..8023e86583885 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -18,12 +18,10 @@ #include "obstacle_stop_planner/planner_data.hpp" -#include -#include -#include #include #include +#include #include #include #include @@ -31,6 +29,7 @@ #include #include +#include #include #include @@ -57,6 +56,9 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using PointVariant = boost::variant; boost::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, @@ -79,24 +81,21 @@ bool isInFrontOfTargetPoint(const Pose & pose, const Point & point); bool checkValidIndex(const Pose & p_base, const Pose & p_next, const Pose & p_target); bool withinPolygon( - const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Polygon2d & boost_polygon, const double radius, const Point2d & prev_point, const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, PointCloud::Ptr within_points_ptr); bool withinPolyhedron( - const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Polygon2d & boost_polygon, const double radius, const Point2d & prev_point, const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, PointCloud::Ptr within_points_ptr, double z_min, double z_max); -bool convexHull( - const std::vector & pointcloud, std::vector & polygon_points); +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point); void createOneStepPolygon( - const Pose & base_step_pose, const Pose & next_step_pose, std::vector & polygon, + const Pose & base_step_pose, const Pose & next_step_pose, Polygon2d & hull_polygon, const VehicleInfo & vehicle_info, const double expand_width = 0.0); -bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose); - void getNearestPoint( const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); @@ -105,12 +104,12 @@ void getLateralNearestPoint( const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, double * deviation); -void getNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * nearest_collision_point, - rclcpp::Time * nearest_collision_point_time); +double getNearestPointAndDistanceForPredictedObject( + const geometry_msgs::msg::PoseArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point); void getLateralNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * lateral_nearest_point, + const PoseArray & object, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, double * deviation); Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); @@ -141,6 +140,17 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e TrajectoryPoint getExtendTrajectoryPoint( double extend_distance, const TrajectoryPoint & goal_point); +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max); + +pcl::PointXYZ pointToPcl(const double x, const double y, const double z); + +boost::optional getObstacleFromUuid( + const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id); + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); + rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index f3311521103e8..36ffca308981f 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -9,6 +9,7 @@ Taiki Tanaka Tomoya Kimura Shumpei Wakabayashi + Berkay Karaman Apache License 2.0 @@ -16,14 +17,12 @@ Yukihiro Saito ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs diagnostic_msgs - libopencv-dev motion_utils nav_msgs pcl_conversions diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 83c82c602c93f..bc3e07cf8dd74 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -273,13 +273,75 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( *need_to_stop = false; } +void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header, + const PredictedObject & target_object) +{ + debug_values_.data.clear(); + debug_values_.data.resize(num_debug_values_, 0.0); + + const double current_velocity = current_odometry_ptr->twist.twist.linear.x; + double col_point_distance; + double point_velocity; + /* + * calc distance to collision point + */ + calcDistanceToNearestPointOnPath( + trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point, + nearest_collision_point_time, &col_point_distance, trajectory_header); + + /* + * calc yaw of trajectory at collision point + */ + const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx); + + /* + * estimate velocity of collision point + */ + calculateProjectedVelocityFromObject(target_object, traj_yaw, &point_velocity); + + // calculate max(target) velocity of self + const double upper_velocity = + calcUpperVelocity(col_point_distance, point_velocity, current_velocity); + pub_debug_->publish(debug_values_); + + if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // if the target object is obstacle return stop true + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Target object is pedestrian. Insert stop line."); + *need_to_stop = true; + return; + } + + if (upper_velocity <= param_.thresh_vel_to_stop) { + // if upper velocity is too low, need to stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Upper velocity is too low. Insert stop line."); + *need_to_stop = true; + return; + } + + /* + * insert max velocity + */ + insertMaxVelocityToPath( + self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); + *need_to_stop = false; +} + void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const TrajectoryPoints & trajectory, const int nearest_point_idx, const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, double * distance, const std_msgs::msg::Header & trajectory_header) { - if (trajectory.size() == 0) { + if (trajectory.empty()) { RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "input path is too short(size=0)"); @@ -376,6 +438,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( } } +void AdaptiveCruiseController::calculateProjectedVelocityFromObject( + const PredictedObject & object, const double traj_yaw, double * velocity) +{ + /* get object velocity, and current yaw */ + double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); + + *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw)); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; +} + bool AdaptiveCruiseController::estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, double * velocity) diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index e12d4f1296cbc..15b0a721ed170 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -53,12 +53,12 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const std::vector & polygon, const double z, const PolygonType & type) + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; - for (const auto & point : polygon) { + for (const auto & point : polygon.outer()) { Eigen::Vector3d eigen_point; - eigen_point << point.x, point.y, z; + eigen_point << point.x(), point.y(), z; eigen_polygon.push_back(eigen_point); } return pushPolygon(eigen_polygon, type); @@ -99,13 +99,13 @@ bool ObstacleStopPlannerDebugNode::pushPolygon( } bool ObstacleStopPlannerDebugNode::pushPolyhedron( - const std::vector & polyhedron, const double z_min, const double z_max, + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; - for (const auto & point : polyhedron) { - eigen_polyhedron.emplace_back(point.x, point.y, z_min); - eigen_polyhedron.emplace_back(point.x, point.y, z_max); + for (const auto & point : polyhedron.outer()) { + eigen_polyhedron.emplace_back(point.x(), point.y(), z_min); + eigen_polyhedron.emplace_back(point.x(), point.y(), z_max); } return pushPolyhedron(eigen_polyhedron, type); diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index fca182525677c..54713250c9200 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -24,8 +23,8 @@ #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/planner_utils.hpp" -#include -#include +#include +#include #include #include @@ -41,6 +40,7 @@ namespace motion_planning { +using autoware_auto_perception_msgs::msg::PredictedObject; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; @@ -76,6 +76,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.voxel_grid_z = declare_parameter("voxel_grid_z"); p.use_predicted_objects = declare_parameter("use_predicted_objects"); p.publish_obstacle_polygon = declare_parameter("publish_obstacle_polygon"); + p.predicted_object_filtering_threshold = + declare_parameter("predicted_object_filtering_threshold"); } { @@ -169,6 +171,22 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); } + if (node_param_.use_predicted_objects) { + // Search the maximum lateral margin + std::vector lateral_margins{ + stop_param_.pedestrian_lateral_margin, stop_param_.vehicle_lateral_margin, + stop_param_.unknown_lateral_margin}; + if (node_param_.enable_slow_down) { + lateral_margins.push_back(slow_down_param_.pedestrian_lateral_margin); + lateral_margins.push_back(slow_down_param_.vehicle_lateral_margin); + lateral_margins.push_back(slow_down_param_.unknown_lateral_margin); + } + const double max_lateral_margin = + *std::max_element(lateral_margins.begin(), lateral_margins.end()); + object_filtering_margin_ = + max_lateral_margin + node_param_.predicted_object_filtering_threshold; + } + // Initializer acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); @@ -192,10 +210,13 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod this->create_publisher("~/debug/collision_pointcloud", 1); // Subscribers - sub_point_cloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&ObstacleStopPlannerNode::onPointCloud, this, std::placeholders::_1), - createSubscriptionOptions(this)); + if (!node_param_.use_predicted_objects) { + // No need to point cloud while using predicted objects + sub_point_cloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&ObstacleStopPlannerNode::onPointCloud, this, std::placeholders::_1), + createSubscriptionOptions(this)); + } sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, @@ -273,7 +294,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m return; } - if (!obstacle_ros_pointcloud_ptr) { + if (!obstacle_ros_pointcloud_ptr && !node_param_.use_predicted_objects) { waiting("obstacle pointcloud"); return; } @@ -328,7 +349,9 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m base_trajectory, stop_param.step_length, planner_data.decimate_trajectory_index_map); if (node_param_.use_predicted_objects) { - searchPredictedObject(decimate_trajectory, planner_data, vehicle_info, stop_param); + searchPredictedObject( + decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info, + stop_param); } else { // search obstacles within slow-down/collision area searchObstacle( @@ -358,7 +381,6 @@ void ObstacleStopPlannerNode::searchObstacle( PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr) { - const auto object_ptr = object_ptr_; // search candidate obstacle pointcloud PointCloud::Ptr slow_down_pointcloud_ptr(new PointCloud); PointCloud::Ptr obstacle_candidate_pointcloud_ptr(new PointCloud); @@ -385,7 +407,7 @@ void ObstacleStopPlannerNode::searchObstacle( const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); if (node_param_.enable_slow_down) { - std::vector one_step_move_slow_down_range_polygon; + Polygon2d one_step_move_slow_down_range_polygon; // create one step polygon for slow_down range createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, @@ -428,7 +450,7 @@ void ObstacleStopPlannerNode::searchObstacle( } { - std::vector one_step_move_vehicle_polygon; + Polygon2d one_step_move_vehicle_polygon; // create one step polygon for vehicle createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin); @@ -461,22 +483,6 @@ void ObstacleStopPlannerNode::searchObstacle( *collision_pointcloud_ptr, p_front, &nearest_collision_point, &nearest_collision_point_time); - if (node_param_.enable_z_axis_obstacle_filtering) { - debug_ptr_->pushPolyhedron( - one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Collision); - if ( - (pub_collision_pointcloud_debug_->get_subscription_count() + - pub_collision_pointcloud_debug_->get_intra_process_subscription_count()) > 0) { - auto obstacle_ros_pointcloud_debug_ptr = std::make_shared(); - pcl::toROSMsg(*collision_pointcloud_ptr, *obstacle_ros_pointcloud_debug_ptr); - obstacle_ros_pointcloud_debug_ptr->header.frame_id = trajectory_header.frame_id; - pub_collision_pointcloud_debug_->publish(*obstacle_ros_pointcloud_debug_ptr); - } - } else { - debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - } obstacle_history_.emplace_back(now, nearest_collision_point); break; @@ -485,6 +491,12 @@ void ObstacleStopPlannerNode::searchObstacle( } for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + const auto old_point_cloud_ptr = getOldPointCloudPtr(); + if (old_point_cloud_ptr->empty()) { + // no need to check collision + break; + } + // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; const auto & p_back = decimate_trajectory.at(i + 1).pose; @@ -496,19 +508,11 @@ void ObstacleStopPlannerNode::searchObstacle( const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); - std::vector one_step_move_vehicle_polygon; + Polygon2d one_step_move_vehicle_polygon; // create one step polygon for vehicle createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin); - if (node_param_.enable_z_axis_obstacle_filtering) { - debug_ptr_->pushPolyhedron( - one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Vehicle); - } else { - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Vehicle); - } - PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; @@ -516,11 +520,11 @@ void ObstacleStopPlannerNode::searchObstacle( if (node_param_.enable_z_axis_obstacle_filtering) { planner_data.found_collision_points = withinPolyhedron( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, - next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr, z_axis_min, z_axis_max); + next_center_point, old_point_cloud_ptr, collision_pointcloud_ptr, z_axis_min, z_axis_max); } else { planner_data.found_collision_points = withinPolygon( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, - next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr); + next_center_point, old_point_cloud_ptr, collision_pointcloud_ptr); } if (planner_data.found_collision_points) { @@ -550,6 +554,7 @@ void ObstacleStopPlannerNode::searchObstacle( mutex_.lock(); const auto current_odometry_ptr = current_odometry_ptr_; + const auto object_ptr = object_ptr_; mutex_.unlock(); acc_controller_->insertAdaptiveCruiseVelocity( @@ -561,20 +566,26 @@ void ObstacleStopPlannerNode::searchObstacle( if (!planner_data.stop_require) { obstacle_history_.clear(); } - break; } } } void ObstacleStopPlannerNode::searchPredictedObject( - const TrajectoryPoints & decimate_trajectory, PlannerData & planner_data, - const VehicleInfo & vehicle_info, const StopParam & stop_param) + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param) { + mutex_.lock(); const auto object_ptr = object_ptr_; - if (object_ptr->objects.empty()) { - return; - } + const auto current_odometry_pointer = current_odometry_ptr_; + mutex_.unlock(); + + const auto ego_pose = current_odometry_pointer->pose.pose; + PredictedObjects filtered_objects; + filterObstacles( + *object_ptr.get(), ego_pose, decimate_trajectory, object_filtering_margin_, filtered_objects); + const auto now = this->now(); updatePredictedObstacleHistory(now); @@ -582,499 +593,388 @@ void ObstacleStopPlannerNode::searchPredictedObject( // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; const auto & p_back = decimate_trajectory.at(i + 1).pose; - const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); - const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); - const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + const auto z_axis_min = p_front.position.z; + const auto z_axis_max = + p_front.position.z + vehicle_info.vehicle_height_m + node_param_.z_axis_filtering_buffer; if (node_param_.enable_slow_down) { - for (const auto & obj : object_ptr->objects) { + double min_slow_down_norm = 0.0; + bool is_init = false; + size_t nearest_slow_down_object_index = 0; + geometry_msgs::msg::Point nearest_slow_down_point; + geometry_msgs::msg::PoseArray slow_down_points; + + for (size_t j = 0; j < filtered_objects.objects.size(); ++j) { + const auto & obj = filtered_objects.objects.at(j); + if (node_param_.enable_z_axis_obstacle_filtering) { + if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) { + continue; + } + } + Polygon2d one_step_move_slow_down_range; + bool found_slow_down_object = false; + Polygon2d object_polygon{}; if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - Polygon2d object_polygon{}; object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - - std::vector one_step_move_slow_down_range_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for slow_down range createOneStepPolygon( - p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, + p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.pedestrian_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); - for (const auto & point : one_step_move_slow_down_range_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - planner_data.found_slow_down_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); - - const auto found_first_slow_down_points = - planner_data.found_slow_down_points && !planner_data.slow_down_require; - - if (found_first_slow_down_points) { - // found nearest slow down obstacle - planner_data.decimate_trajectory_slow_down_index = i; - planner_data.slow_down_require = true; - std::vector slow_down_point; - geometry_msgs::msg::PoseArray slow_down_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, slow_down_point); - for (const auto & point : slow_down_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - slow_down_points.poses.push_back(pose); - } - - getNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.nearest_slow_down_point_pose, - &planner_data.nearest_collision_point_time); - - getLateralNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point_pose, - &planner_data.lateral_deviation); - - debug_ptr_->pushObstaclePoint( - planner_data.nearest_slow_down_point_pose.position, PointType::SlowDown); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); - } + found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_range, vehicle_info, + slow_down_param_.vehicle_lateral_margin); const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - Polygon2d object_polygon{}; object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - std::vector one_step_move_slow_down_range_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for slow_down range + found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); + + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( - p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, - slow_down_param_.vehicle_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); - for (const auto & point : one_step_move_slow_down_range_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - planner_data.found_slow_down_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); - - const auto found_first_slow_down_points = - planner_data.found_slow_down_points && !planner_data.slow_down_require; - - if (found_first_slow_down_points) { - // found nearest slow down obstacle - planner_data.decimate_trajectory_slow_down_index = i; - planner_data.slow_down_require = true; - std::vector slow_down_point; - geometry_msgs::msg::PoseArray slow_down_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, slow_down_point); - for (const auto & point : slow_down_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - slow_down_points.poses.push_back(pose); - } + p_front, p_back, one_step_move_slow_down_range, vehicle_info, + slow_down_param_.unknown_lateral_margin); - getNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.nearest_slow_down_point_pose, - &planner_data.nearest_collision_point_time); + object_polygon = convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - getLateralNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point_pose, - &planner_data.lateral_deviation); + found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - debug_ptr_->pushObstaclePoint( - planner_data.nearest_slow_down_point_pose.position, PointType::SlowDown); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + continue; + } + if (found_slow_down_object) { + geometry_msgs::msg::PoseArray slow_down_points_tmp; + + std::vector slow_down_point; + bg::intersection(one_step_move_slow_down_range, object_polygon, slow_down_point); + for (const auto & point : slow_down_point) { + geometry_msgs::msg::Pose pose; + pose.position.x = point.x(); + pose.position.y = point.y(); + slow_down_points_tmp.poses.push_back(pose); } - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - Polygon2d object_polygon{}; - object_polygon = convertPolygonObjectToGeometryPolygon( - obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - std::vector one_step_move_slow_down_range_polygon; - Polygon2d one_step_move_slow_down_range_polygon2d; - // create one step polygon for slow_down range - createOneStepPolygon( - p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, - slow_down_param_.unknown_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); - - const auto found_first_slow_down_points = - planner_data.found_slow_down_points && !planner_data.slow_down_require; - - if (found_first_slow_down_points) { - // found nearest slow down obstacle - planner_data.decimate_trajectory_slow_down_index = i; - planner_data.slow_down_require = true; - std::vector slow_down_point; - geometry_msgs::msg::PoseArray slow_down_points; - bg::intersection( - one_step_move_slow_down_range_polygon2d, object_polygon, slow_down_point); - for (const auto & point : slow_down_point) { + // Also check the corner points + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_slow_down_range)) { geometry_msgs::msg::Pose pose; pose.position.x = point.x(); pose.position.y = point.y(); - slow_down_points.poses.push_back(pose); + slow_down_points_tmp.poses.push_back(pose); } + } + geometry_msgs::msg::Point nearest_slow_down_point_tmp; + const double norm = getNearestPointAndDistanceForPredictedObject( + slow_down_points_tmp, p_front, &nearest_slow_down_point_tmp); + if (norm < min_slow_down_norm || !is_init) { + min_slow_down_norm = norm; + nearest_slow_down_point = nearest_slow_down_point_tmp; + is_init = true; + nearest_slow_down_object_index = j; + slow_down_points = slow_down_points_tmp; + } + } + } - getNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.nearest_slow_down_point_pose, - &planner_data.nearest_collision_point_time); + planner_data.found_slow_down_points = is_init; - getLateralNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point_pose, - &planner_data.lateral_deviation); + const auto found_first_slow_down_points = + planner_data.found_slow_down_points && !planner_data.slow_down_require; - debug_ptr_->pushObstaclePoint( - planner_data.nearest_slow_down_point_pose.position, PointType::SlowDown); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); - } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); + if (found_first_slow_down_points) { + // found nearest slow down obstacle + planner_data.decimate_trajectory_slow_down_index = i; + planner_data.slow_down_require = true; + planner_data.nearest_slow_down_point = + pointToPcl(nearest_slow_down_point.x, nearest_slow_down_point.y, p_front.position.z); + planner_data.nearest_collision_point_time = filtered_objects.header.stamp; + planner_data.slow_down_object_shape = + filtered_objects.objects.at(nearest_slow_down_object_index).shape; + + // TODO(brkay54): lateral_nearest_slow_down_point_pose and nearest_slow_down_point_pose are + // not used + getLateralNearestPointForPredictedObject( + slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point, + &planner_data.lateral_deviation); + + // Push slow down debugging points + Polygon2d one_step_move_slow_down_vehicle_polygon; + + const auto & obj = filtered_objects.objects.at(nearest_slow_down_object_index); + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, + slow_down_param_.pedestrian_lateral_margin); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, + slow_down_param_.vehicle_lateral_margin); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, + slow_down_param_.unknown_lateral_margin); } + debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown); + + debug_ptr_->pushPolygon( + one_step_move_slow_down_vehicle_polygon, p_front.position.z, PolygonType::SlowDown); + } else { + // only used for pedestrian and debugging + Polygon2d one_step_move_slow_down_range_dbg; + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_range_dbg, vehicle_info, + slow_down_param_.pedestrian_lateral_margin); + + debug_ptr_->pushPolygon( + one_step_move_slow_down_range_dbg, p_front.position.z, PolygonType::SlowDownRange); } } { - for (const auto & obj : object_ptr->objects) { + double min_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; + geometry_msgs::msg::Point nearest_collision_point; + + for (size_t j = 0; j < filtered_objects.objects.size(); ++j) { + const auto & obj = filtered_objects.objects.at(j); + if (node_param_.enable_z_axis_obstacle_filtering) { + if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) { + continue; + } + } + Polygon2d one_step_move_collision_polygon; + bool found_collision_object = false; + Polygon2d object_polygon{}; if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - Polygon2d object_polygon{}; object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - std::vector one_step_move_vehicle_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + p_front, p_back, one_step_move_collision_polygon, vehicle_info, stop_param.pedestrian_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - for (const auto & point : one_step_move_vehicle_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - const auto found_collision_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); - - if (found_collision_points) { - Pose nearest_collision_point_pose; - rclcpp::Time nearest_collision_point_time; - - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &nearest_collision_point_pose, - &nearest_collision_point_time); - predicted_object_history_.emplace_back(now, nearest_collision_point_pose); - break; - } + found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - Polygon2d object_polygon{}; object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - - std::vector one_step_move_vehicle_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + p_front, p_back, one_step_move_collision_polygon, vehicle_info, stop_param.vehicle_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - for (const auto & point : one_step_move_vehicle_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - const auto found_collision_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); - - if (found_collision_points) { - Pose nearest_collision_point_pose; - rclcpp::Time nearest_collision_point_time; - - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &nearest_collision_point_pose, - &nearest_collision_point_time); - predicted_object_history_.emplace_back(now, nearest_collision_point_pose); - break; - } + found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - Polygon2d object_polygon{}; object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - - std::vector one_step_move_vehicle_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + p_front, p_back, one_step_move_collision_polygon, vehicle_info, stop_param.unknown_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - for (const auto & point : one_step_move_vehicle_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - const auto found_collision_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + continue; + } + if (found_collision_object) { + geometry_msgs::msg::PoseArray collision_points_tmp; - if (found_collision_points) { - Pose nearest_collision_point_pose; - rclcpp::Time nearest_collision_point_time; + std::vector collision_point; + bg::intersection(one_step_move_collision_polygon, object_polygon, collision_point); + for (const auto & point : collision_point) { + geometry_msgs::msg::Pose pose; + pose.position.x = point.x(); + pose.position.y = point.y(); + collision_points_tmp.poses.push_back(pose); + } - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_point); - for (const auto & point : collision_point) { + // Also check the corner points + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_collision_polygon)) { geometry_msgs::msg::Pose pose; pose.position.x = point.x(); pose.position.y = point.y(); - collision_points.poses.push_back(pose); + collision_points_tmp.poses.push_back(pose); } - getNearestPointForPredictedObject( - collision_points, p_front, &nearest_collision_point_pose, - &nearest_collision_point_time); - - predicted_object_history_.emplace_back(now, nearest_collision_point_pose); - break; } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); + geometry_msgs::msg::Point nearest_collision_point_tmp; + const double norm = getNearestPointAndDistanceForPredictedObject( + collision_points_tmp, p_front, &nearest_collision_point_tmp); + if (norm < min_collision_norm || !is_init) { + min_collision_norm = norm; + nearest_collision_point = nearest_collision_point_tmp; + is_init = true; + nearest_collision_object_index = j; + } } } + if (is_init) { + predicted_object_history_.emplace_back( + now, nearest_collision_point, + filtered_objects.objects.at(nearest_collision_object_index)); + break; + } + + // only used for pedestrian + Polygon2d one_step_move_collision_dbg; + createOneStepPolygon( + p_front, p_back, one_step_move_collision_dbg, vehicle_info, + stop_param.pedestrian_lateral_margin); + if (node_param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_collision_dbg, z_axis_min, z_axis_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_collision_dbg, p_front.position.z, PolygonType::Vehicle); + } } } + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + if (predicted_object_history_.empty()) { + break; + } + // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; const auto & p_back = decimate_trajectory.at(i + 1).pose; - const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); - const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); - const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); - for (const auto & obj : object_ptr->objects) { - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - Polygon2d object_polygon{}; - object_polygon = convertCylindricalObjectToGeometryPolygon( - obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + const auto z_axis_min = p_front.position.z; + const auto z_axis_max = + p_front.position.z + vehicle_info.vehicle_height_m + node_param_.z_axis_filtering_buffer; - std::vector one_step_move_vehicle_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for vehicle + double min_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; + + for (size_t j = 0; j < predicted_object_history_.size(); ++j) { + // check new collision points + const auto & obj = predicted_object_history_.at(j).object; + if (node_param_.enable_z_axis_obstacle_filtering) { + if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) { + continue; + } + } + Point2d collision_point; + collision_point.x() = predicted_object_history_.at(j).point.x; + collision_point.y() = predicted_object_history_.at(j).point.y; + Polygon2d one_step_move_vehicle_polygon; + // create one step polygon for vehicle + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.pedestrian_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - for (const auto & point : one_step_move_vehicle_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - // check new collision points - planner_data.found_collision_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.vehicle_lateral_margin); - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - rclcpp::Time nearest_collision_point_time; + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.unknown_lateral_margin); - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &planner_data.nearest_collision_point_pose, - &nearest_collision_point_time); - - debug_ptr_->pushObstaclePoint( - planner_data.nearest_collision_point_pose.position, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - one_step_move_vehicle_polygon.clear(); - if (node_param_.publish_obstacle_polygon) { - std::vector obstacle_polygon; - for (const auto & point : object_polygon.outer()) { - obstacle_polygon.emplace_back(point.x(), point.y()); - } - debug_ptr_->pushPolygon(obstacle_polygon, p_front.position.z, PolygonType::Obstacle); - } - planner_data.stop_require = planner_data.found_collision_points; - mutex_.lock(); - const auto current_odometry_ptr = current_odometry_ptr_; - mutex_.unlock(); - if (!planner_data.stop_require) { - predicted_object_history_.clear(); - } - break; + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + continue; + } + if (bg::within(collision_point, one_step_move_vehicle_polygon)) { + const double norm = calcDistance2d(predicted_object_history_.at(j).point, p_front.position); + if (norm < min_collision_norm || !is_init) { + min_collision_norm = norm; + is_init = true; + nearest_collision_object_index = j; } + } + } + + planner_data.found_collision_points = is_init; + + if (planner_data.found_collision_points) { + planner_data.nearest_collision_point = pointToPcl( + predicted_object_history_.at(nearest_collision_object_index).point.x, + predicted_object_history_.at(nearest_collision_object_index).point.y, p_front.position.z); + + planner_data.decimate_trajectory_collision_index = i; + + planner_data.nearest_collision_point_time = + predicted_object_history_.at(nearest_collision_object_index).detection_time; + + // create one step polygon for vehicle collision debug + Polygon2d one_step_move_vehicle_polygon; + Polygon2d object_polygon{}; + + const auto & obj = predicted_object_history_.at(nearest_collision_object_index).object; + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.pedestrian_lateral_margin); + object_polygon = convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.vehicle_lateral_margin); const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - Polygon2d object_polygon{}; object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - std::vector one_step_move_vehicle_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for vehicle + + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.unknown_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - for (const auto & point : one_step_move_vehicle_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - // check new collision points - planner_data.found_collision_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); - - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - rclcpp::Time nearest_collision_point_time; - - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &planner_data.nearest_collision_point_pose, - &nearest_collision_point_time); - - debug_ptr_->pushObstaclePoint( - planner_data.nearest_collision_point_pose.position, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - one_step_move_vehicle_polygon.clear(); - if (node_param_.publish_obstacle_polygon) { - std::vector obstacle_polygon; - for (const auto & point : object_polygon.outer()) { - obstacle_polygon.emplace_back(point.x(), point.y()); - } - debug_ptr_->pushPolygon(obstacle_polygon, p_front.position.z, PolygonType::Obstacle); - } - - planner_data.stop_require = planner_data.found_collision_points; - mutex_.lock(); - const auto current_odometry_ptr = current_odometry_ptr_; - mutex_.unlock(); - if (!planner_data.stop_require) { - predicted_object_history_.clear(); - } - break; - } - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - Polygon2d object_polygon{}; object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - std::vector one_step_move_vehicle_polygon; - Polygon2d one_step_move_vehicle_polygon2d; - // create one step polygon for vehicle - createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, - stop_param.vehicle_lateral_margin); + if (node_param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Collision); + } else { debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + } - for (const auto & point : one_step_move_vehicle_polygon) { - one_step_move_vehicle_polygon2d.outer().push_back(Point2d(point.x, point.y)); - } - // check new collision points - planner_data.found_collision_points = - bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + if (node_param_.publish_obstacle_polygon) { + debug_ptr_->pushPolygon(object_polygon, p_front.position.z, PolygonType::Obstacle); + } - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - rclcpp::Time nearest_collision_point_time; + planner_data.stop_require = planner_data.found_collision_points; + mutex_.lock(); + const auto current_odometry_ptr = current_odometry_ptr_; + const auto latest_object_ptr = object_ptr_; + mutex_.unlock(); + // find latest state of predicted object to get latest velocity and acceleration values + auto obj_latest_state = getObstacleFromUuid(*latest_object_ptr, obj.object_id); + if (!obj_latest_state) { + // Can not find the object in the latest object list. Send previous state. + obj_latest_state = boost::make_optional(obj); + } - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &planner_data.nearest_collision_point_pose, - &nearest_collision_point_time); - - debug_ptr_->pushObstaclePoint( - planner_data.nearest_collision_point_pose.position, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - one_step_move_vehicle_polygon.clear(); - if (node_param_.publish_obstacle_polygon) { - std::vector obstacle_polygon; - for (const auto & point : object_polygon.outer()) { - obstacle_polygon.emplace_back(point.x(), point.y()); - } - debug_ptr_->pushPolygon(obstacle_polygon, p_front.position.z, PolygonType::Obstacle); - } - planner_data.stop_require = planner_data.found_collision_points; - mutex_.lock(); - const auto current_odometry_ptr = current_odometry_ptr_; - mutex_.unlock(); - if (!planner_data.stop_require) { - predicted_object_history_.clear(); - } + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, current_odometry_ptr, &planner_data.stop_require, + &output, trajectory_header, *obj_latest_state); - break; - } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); + if (!planner_data.stop_require) { + predicted_object_history_.clear(); } + break; } } } @@ -1093,18 +993,11 @@ void ObstacleStopPlannerNode::insertVelocity( planner_data.decimate_trajectory_collision_index) + planner_data.trajectory_trim_index; boost::optional> index_with_dist_remain; - if (node_param_.use_predicted_objects) { - index_with_dist_remain = findNearestFrontIndex( - std::min(idx, traj_end_idx), output, - createPoint( - planner_data.nearest_collision_point_pose.position.x, - planner_data.nearest_collision_point_pose.position.y, 0)); - } else { - index_with_dist_remain = findNearestFrontIndex( - std::min(idx, traj_end_idx), output, - createPoint( - planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); - } + + index_with_dist_remain = findNearestFrontIndex( + std::min(idx, traj_end_idx), output, + createPoint( + planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); if (index_with_dist_remain) { const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx); @@ -1129,6 +1022,10 @@ void ObstacleStopPlannerNode::insertVelocity( return 0.0; } + if (ego_seg_idx == output.size() - 1) { + return 0.0; + } + size_t stop_seg_idx = 0; if (stop_point.index < output.size() - 1) { const double lon_offset = @@ -1223,30 +1120,34 @@ void ObstacleStopPlannerNode::insertVelocity( double slow_down_velocity; if (node_param_.use_predicted_objects) { - for (const auto & obj : object_ptr_->objects) { - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - slow_down_velocity = - slow_down_param_.min_slow_down_velocity + - (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * - std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.pedestrian_lateral_margin; - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - slow_down_velocity = - slow_down_param_.min_slow_down_velocity + - (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * - std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.vehicle_lateral_margin; - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - slow_down_velocity = - slow_down_param_.min_slow_down_velocity + - (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * - std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.unknown_lateral_margin; - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); - } + if ( + planner_data.slow_down_object_shape.type == + autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + slow_down_velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.pedestrian_lateral_margin; + } else if ( + planner_data.slow_down_object_shape.type == + autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + slow_down_velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.vehicle_lateral_margin; + } else if ( + planner_data.slow_down_object_shape.type == + autoware_auto_perception_msgs::msg::Shape::POLYGON) { + slow_down_velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.unknown_lateral_margin; + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + planner_data.slow_down_object_shape.type); } } else { slow_down_velocity = @@ -1632,6 +1533,46 @@ void ObstacleStopPlannerNode::resetExternalVelocityLimit( RCLCPP_INFO(get_logger(), "reset velocity limit"); } +void ObstacleStopPlannerNode::filterObstacles( + const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj, + const double dist_threshold, PredictedObjects & filtered_objects) +{ + filtered_objects.header = input_objects.header; + + for (auto & object : input_objects.objects) { + // Check is it in front of ego vehicle + if (!isFrontObstacle(ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) { + continue; + } + + // Check is it near to trajectory + const double max_length = calcObstacleMaxLength(object.shape); + const size_t seg_idx = motion_utils::findNearestSegmentIndex( + traj, object.kinematics.initial_pose_with_covariance.pose.position); + const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); + const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; + + if (seg_idx == traj.size() - 2) { + // Calculate longitudinal offset + const auto longitudinal_dist = std::abs(segment_vec.dot(target_vec) / segment_vec.norm()); + if ( + longitudinal_dist - max_length - vehicle_info_.max_longitudinal_offset_m - dist_threshold > + 0.0) { + continue; + } + } + const auto lateral_dist = std::abs(segment_vec.cross(target_vec)(2) / segment_vec.norm()); + if (lateral_dist - max_length - vehicle_info_.max_lateral_offset_m - dist_threshold > 0.0) { + continue; + } + PredictedObject filtered_object = object; + filtered_objects.objects.push_back(filtered_object); + } +} + void ObstacleStopPlannerNode::publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel) { diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 5fba295e3e1d2..45cced4599602 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -27,6 +27,8 @@ namespace motion_planning { +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -258,16 +260,11 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } bool withinPolygon( - const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Polygon2d & boost_polygon, const double radius, const Point2d & prev_point, const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, PointCloud::Ptr within_points_ptr) { - Polygon2d boost_polygon; bool find_within_points = false; - for (const auto & point : cv_polygon) { - boost_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); for (size_t j = 0; j < candidate_points_ptr->size(); ++j) { Point2d point(candidate_points_ptr->at(j).x, candidate_points_ptr->at(j).y); @@ -282,16 +279,11 @@ bool withinPolygon( } bool withinPolyhedron( - const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Polygon2d & boost_polygon, const double radius, const Point2d & prev_point, const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, PointCloud::Ptr within_points_ptr, double z_min, double z_max) { - Polygon2d boost_polygon; bool find_within_points = false; - for (const auto & point : cv_polygon) { - boost_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); for (const auto & candidate_point : *candidate_points_ptr) { Point2d point(candidate_point.x, candidate_point.y); @@ -307,79 +299,62 @@ bool withinPolyhedron( return find_within_points; } -bool convexHull( - const std::vector & pointcloud, std::vector & polygon_points) +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { - cv::Point2d centroid; - centroid.x = 0; - centroid.y = 0; - for (const auto & point : pointcloud) { - centroid.x += point.x; - centroid.y += point.y; - } - centroid.x = centroid.x / static_cast(pointcloud.size()); - centroid.y = centroid.y / static_cast(pointcloud.size()); - - std::vector normalized_pointcloud; - std::vector normalized_polygon_points; - for (const auto & p : pointcloud) { - normalized_pointcloud.emplace_back( - cv::Point((p.x - centroid.x) * 1000.0, (p.y - centroid.y) * 1000.0)); - } - cv::convexHull(normalized_pointcloud, normalized_polygon_points); + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; - for (const auto & p : normalized_polygon_points) { - cv::Point2d polygon_point; - polygon_point.x = (p.x / 1000.0 + centroid.x); - polygon_point.y = (p.y / 1000.0 + centroid.y); - polygon_points.push_back(polygon_point); - } - return true; + boost::geometry::append(polygon.outer(), point); } void createOneStepPolygon( - const Pose & base_step_pose, const Pose & next_step_pose, std::vector & polygon, + const Pose & base_step_pose, const Pose & next_step_pose, Polygon2d & hull_polygon, const VehicleInfo & vehicle_info, const double expand_width) { - std::vector one_step_move_vehicle_corner_points; - - const auto & i = vehicle_info; - const auto & front_m = i.max_longitudinal_offset_m; - const auto & width_m = i.vehicle_width_m / 2.0 + expand_width; - const auto & back_m = i.rear_overhang_m; - // start step - { - const auto yaw = getRPY(base_step_pose).z; - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, - base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, - base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, - base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, - base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); - } - // next step - { - const auto yaw = getRPY(next_step_pose).z; - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, - next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, - next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, - next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, - next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); - } - convexHull(one_step_move_vehicle_corner_points, polygon); + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + { // base step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + } + + { // next step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + } + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + boost::geometry::convex_hull(polygon, hull_polygon); } void insertStopPoint( @@ -480,9 +455,9 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e } const auto goal_point = input.back(); - double interpolation_distance = 0.1; + constexpr double interpolation_distance = 0.1; - double extend_sum = 0.0; + double extend_sum = interpolation_distance; while (extend_sum <= (extend_distance - interpolation_distance)) { const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); output.push_back(extend_trajectory_point); @@ -494,23 +469,29 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e return output; } -bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose) -{ - try { - TransformStamped transform; - transform = tf_buffer.lookupTransform( - header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds(0.1)); - self_pose.position.x = transform.transform.translation.x; - self_pose.position.y = transform.transform.translation.y; - self_pose.position.z = transform.transform.translation.z; - self_pose.orientation.x = transform.transform.rotation.x; - self_pose.orientation.y = transform.transform.rotation.y; - self_pose.orientation.z = transform.transform.rotation.z; - self_pose.orientation.w = transform.transform.rotation.w; - return true; - } catch (tf2::TransformException & ex) { - return false; - } +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance; + const auto & obj_height = object.shape.dimensions.z; + return obj_pose.pose.position.z - obj_height / 2.0 <= z_max && + obj_pose.pose.position.z + obj_height / 2.0 >= z_min; +} + +pcl::PointXYZ pointToPcl(const double x, const double y, const double z) +{ + // Store the point components in a variant + PointVariant x_variant = x; + PointVariant y_variant = y; + PointVariant z_variant = z; + + // Extract the corresponding components from the variant + auto extract_float = [](const auto & variant) { return boost::get(variant); }; + float pcl_x = boost::apply_visitor(extract_float, x_variant); + float pcl_y = boost::apply_visitor(extract_float, y_variant); + float pcl_z = boost::apply_visitor(extract_float, z_variant); + + // Create a new pcl::PointXYZ object + return {pcl_x, pcl_y, pcl_z}; } void getNearestPoint( @@ -554,34 +535,34 @@ void getLateralNearestPoint( *deviation = min_norm; } -void getNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * nearest_collision_point, - rclcpp::Time * nearest_collision_point_time) +double getNearestPointAndDistanceForPredictedObject( + const geometry_msgs::msg::PoseArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point) { double min_norm = 0.0; bool is_init = false; - for (const auto & p : object.poses) { - double norm = calcDistance2d(p, base_pose); + for (const auto & p : points.poses) { + double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; - *nearest_collision_point = p; - *nearest_collision_point_time = (object.header).stamp; + *nearest_collision_point = p.position; is_init = true; } } + return min_norm; } void getLateralNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * lateral_nearest_point, + const PoseArray & object, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, double * deviation) { double min_norm = std::numeric_limits::max(); - for (size_t i = 0; i < object.poses.size(); ++i) { - double norm = calcDistance2d(object.poses.at(i), base_pose); + for (const auto & pose : object.poses) { + double norm = calcDistance2d(pose, base_pose); if (norm < min_norm) { min_norm = norm; - *lateral_nearest_point = object.poses.at(i); + *lateral_nearest_point = pointToPcl(pose.position.x, pose.position.y, base_pose.position.z); } } *deviation = min_norm; @@ -637,7 +618,9 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); object_polygon.outer().push_back(object_polygon.outer().front()); - + object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + ? object_polygon + : tier4_autoware_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -658,7 +641,9 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } object_polygon.outer().push_back(object_polygon.outer().front()); - + object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + ? object_polygon + : tier4_autoware_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -676,7 +661,54 @@ Polygon2d convertPolygonObjectToGeometryPolygon( object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); } object_polygon.outer().push_back(object_polygon.outer().front()); - + object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + ? object_polygon + : tier4_autoware_utils::inverseClockwise(object_polygon); return object_polygon; } + +boost::optional getObstacleFromUuid( + const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id) +{ + const auto itr = std::find_if( + obstacles.objects.begin(), obstacles.objects.end(), [&](PredictedObject predicted_object) { + return predicted_object.object_id == target_object_id; + }); + + if (itr == obstacles.objects.end()) { + return boost::none; + } + return boost::make_optional(*itr); +} + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) +{ + const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + const Eigen::Vector2d obstacle_vec( + obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); + + return base_pose_vec.dot(obstacle_vec) >= 0; +} + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + } // namespace motion_planning 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 288ae1a61ff14..95afdd7f2ea3b 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,12 +21,20 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); +using motion_planning::ObstacleStopPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; - auto test_manager = std::make_shared(); +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory"); + test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory"); + test_manager->setOdometryTopicName("obstacle_stop_planner/input/odometry"); + return test_manager; +} +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; const auto planning_test_utils_dir = @@ -44,8 +52,13 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) obstacle_stop_planner_dir + "/config/adaptive_cruise_control.param.yaml", "--params-file", obstacle_stop_planner_dir + "/config/obstacle_stop_planner.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + return std::make_shared(node_options); +} +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "obstacle_stop_planner/input/odometry"); test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud"); @@ -53,18 +66,42 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects"); test_manager->publishExpandStopRange( test_target_node, "obstacle_stop_planner/input/expand_stop_range"); +} - // set subscriber with topic name: obstacle stop planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); - // set obstacle stop planner's input topic name(this topic is changed to test node): - test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory"); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point test_manager->testWithAbnormalTrajectory(test_target_node); + + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } diff --git a/planning/obstacle_velocity_limiter/CMakeLists.txt b/planning/obstacle_velocity_limiter/CMakeLists.txt index 0f2363b25be45..3d7c24cfb81f1 100644 --- a/planning/obstacle_velocity_limiter/CMakeLists.txt +++ b/planning/obstacle_velocity_limiter/CMakeLists.txt @@ -35,6 +35,7 @@ if(BUILD_TESTING) test/test_obstacles.cpp test/test_collision_distance.cpp test/test_occupancy_grid_utils.cpp + test/test_${PROJECT_NAME}_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} obstacle_velocity_limiter_node diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index a74128f57d6fd..13fac28291c3d 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -100,9 +100,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg); /// @brief validate the inputs of the node - /// @param[in] ego_idx trajectory index closest to the current ego pose /// @return true if the inputs are valid - bool validInputs(const boost::optional & ego_idx); + bool validInputs(); }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index f8cb3051bad90..377f061faffd7 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -23,6 +23,7 @@ motion_utils nav_msgs pcl_ros + planning_test_utils rclcpp rclcpp_components sensor_msgs diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 6144f338159da..a738fff7b0e5f 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -170,10 +170,16 @@ rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParamete void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { + if (!validInputs()) return; const auto t_start = std::chrono::system_clock::now(); const auto ego_idx = motion_utils::findNearestIndex(msg->points, current_odometry_ptr_->pose.pose); - if (!validInputs(ego_idx)) return; + if (!ego_idx) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), rcutils_duration_value_t(1000), + "Cannot calculate ego index on the trajectory"); + return; + } auto original_traj = *msg; if (preprocessing_params_.calculate_steering_angles) calculateSteeringAngles(original_traj, projection_params_.wheel_base); @@ -229,21 +235,18 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr } } -bool ObstacleVelocityLimiterNode::validInputs(const boost::optional & ego_idx) +bool ObstacleVelocityLimiterNode::validInputs() { constexpr auto one_sec = rcutils_duration_value_t(1000); if (!occupancy_grid_ptr_) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), one_sec, "Occupancy grid not yet received"); if (!dynamic_obstacles_ptr_) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), one_sec, "Dynamic obstacles not yet received"); - if (!ego_idx) - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); if (!current_odometry_ptr_) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), one_sec, "Current ego velocity not yet received"); - return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && ego_idx && current_odometry_ptr_; + return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && current_odometry_ptr_; } } // namespace obstacle_velocity_limiter 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 new file mode 100644 index 0000000000000..c2ffaffc75287 --- /dev/null +++ b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp @@ -0,0 +1,103 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "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 + +using obstacle_velocity_limiter::ObstacleVelocityLimiterNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setTrajectorySubscriber("obstacle_velocity_limiter/output/trajectory"); + test_manager->setTrajectoryInputTopicName("obstacle_velocity_limiter/input/trajectory"); + test_manager->setOdometryTopicName("obstacle_velocity_limiter/input/odometry"); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + + const auto obstacle_velocity_limiter_dir = + ament_index_cpp::get_package_share_directory("obstacle_velocity_limiter"); + + node_options.arguments( + {"--ros-args", "--params-file", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + obstacle_velocity_limiter_dir + "/config/default_obstacle_velocity_limiter.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + rclcpp::Node::SharedPtr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishOdometry(test_target_node, "obstacle_velocity_limiter/input/odometry"); + test_manager->publishPointCloud( + test_target_node, "obstacle_velocity_limiter/input/obstacle_pointcloud"); + test_manager->publishOccupancyGrid( + test_target_node, "obstacle_velocity_limiter/input/occupancy_grid"); + test_manager->publishPredictedObjects( + test_target_node, "obstacle_velocity_limiter/input/dynamic_obstacles"); + test_manager->publishMap(test_target_node, "obstacle_velocity_limiter/input/map"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md index 6538bf1db222d..6d4958783f86d 100644 --- a/planning/planning_debug_tools/Readme.md +++ b/planning/planning_debug_tools/Readme.md @@ -185,3 +185,29 @@ PlotYawOverArclength('yaw_occlusion', occlusion_spot, tracker_time) PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_state', tracker_time) ``` + +## Perception reproducer + +This script can overlay the perception results from the rosbag on the planning simulator. + +In detail, the ego pose in the rosbag which is closest to the current ego pose in the simulator is calculated. +The perception results at the timestamp of the closest ego pose is extracted, and published. + +### How to use + +First, launch the planning simulator, and put the ego pose. +Then, run the script according to the following command. + +By designating a rosbag, perception reproducer can be launched. + +```bash +ros2 run planning_debug_tools perception_reproducer.py -b +``` + +You can designate multiple rosbags in the directory. + +```bash +ros2 run planning_debug_tools perception_reproducer.py -b +``` + +Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index 9cee8bfdac066..deadcd54fffdd 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -11,9 +11,9 @@ Takamasa Horibe ament_cmake_auto + autoware_cmake eigen3_cmake_module - autoware_cmake rosidl_default_generators autoware_auto_planning_msgs diff --git a/planning/planning_debug_tools/scripts/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_reproducer.py index 625b88db2f077..14a1813e85cd5 100755 --- a/planning/planning_debug_tools/scripts/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_reproducer.py @@ -24,9 +24,11 @@ from autoware_auto_perception_msgs.msg import DetectedObjects from autoware_auto_perception_msgs.msg import PredictedObjects +from autoware_auto_perception_msgs.msg import TrackedObjects from autoware_auto_perception_msgs.msg import TrafficSignalArray from geometry_msgs.msg import Quaternion from nav_msgs.msg import Odometry +import numpy as np import psutil import rclpy from rclpy.node import Node @@ -112,14 +114,19 @@ def __init__(self, args): ) # publisher - if self.args.predicted_object: + if self.args.detected_object: self.objects_pub = self.create_publisher( - PredictedObjects, "/perception/object_recognition/objects", 1 + DetectedObjects, "/perception/object_recognition/detection/objects", 1 + ) + elif self.args.tracked_object: + self.objects_pub = self.create_publisher( + TrackedObjects, "/perception/object_recognition/tracking/objects", 1 ) else: self.objects_pub = self.create_publisher( - DetectedObjects, "/perception/object_recognition/detection/objects", 1 + PredictedObjects, "/perception/object_recognition/objects", 1 ) + self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) @@ -138,11 +145,11 @@ def __init__(self, args): # load rosbag print("Stared loading rosbag") - if args.bag: - self.load_rosbag(args.bag) - elif args.directory: + if os.path.isdir(args.bag): for bag_file in sorted(os.listdir(args.directory)): self.load_rosbag(args.directory + "/" + bag_file) + else: + self.load_rosbag(args.bag) print("Ended loading rosbag") # wait for ready to publish/subscribe @@ -157,10 +164,12 @@ def on_odom(self, odom): def on_timer(self): # kill node if required kill_process_name = None - if self.args.predicted_object: - kill_process_name = "map_based_prediction" - else: + if self.args.detected_object: kill_process_name = "dummy_perception_publisher_node" + elif self.args.tracked_object: + kill_process_name = "multi_object_tracker" + else: + kill_process_name = "map_based_prediction" if kill_process_name: try: pid = check_output(["pidof", kill_process_name]) @@ -171,7 +180,7 @@ def on_timer(self): timestamp = self.get_clock().now().to_msg() - if not self.args.predicted_object: + if self.args.detected_object: pointcloud_msg = create_empty_pointcloud(timestamp) self.pointcloud_pub.publish(pointcloud_msg) @@ -185,23 +194,47 @@ def on_timer(self): traffic_signals_msg = msgs[1] if objects_msg: objects_msg.header.stamp = timestamp - if not self.args.predicted_object: - objects_msg.header.frame_id = "map" - for o in objects_msg.objects: - object_pose = o.kinematics.pose_with_covariance.pose - ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) - theta = math.atan2(object_pose.position.x, object_pose.position.y) - length = math.hypot(object_pose.position.x, object_pose.position.y) + if self.args.detected_object: + log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) + log_ego_pose_trans_mat = np.array( + [ + [ + math.cos(log_ego_yaw), + -math.sin(log_ego_yaw), + log_ego_pose.position.x, + ], + [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + ego_yaw = get_yaw_from_quaternion(self.ego_pose.orientation) + ego_pose_trans_mat = np.array( + [ + [math.cos(ego_yaw), -math.sin(ego_yaw), self.ego_pose.position.x], + [math.sin(ego_yaw), math.cos(ego_yaw), self.ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) - object_pose.position.x = log_ego_pose.position.x + length * math.cos( - ego_yaw + theta + for o in objects_msg.objects: + log_object_pose = o.kinematics.pose_with_covariance.pose + log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) + log_object_pos_vec = np.array( + [log_object_pose.position.x, log_object_pose.position.y, 1.0] ) - object_pose.position.y = log_ego_pose.position.y + length * math.sin( - ego_yaw + theta + + # translate object pose from ego pose in log to ego pose in simulation + object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( + log_ego_pose_trans_mat.dot(log_object_pos_vec.T) ) - obj_yaw = get_yaw_from_quaternion(object_pose.orientation) - object_pose.orientation = get_quaternion_from_yaw(ego_yaw + obj_yaw) + object_pose = o.kinematics.pose_with_covariance.pose + object_pose.position.x = object_pos_vec[0] + object_pose.position.y = object_pos_vec[1] + object_pose.orientation = get_quaternion_from_yaw( + log_object_yaw + log_ego_yaw - ego_yaw + ) self.objects_pub.publish(objects_msg) if traffic_signals_msg: @@ -256,9 +289,11 @@ def load_rosbag(self, rosbag2_path: str): type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} objects_topic = ( - "/perception/object_recognition/objects" - if self.args.predicted_object - else "/perception/object_recognition/detection/objects" + "/perception/object_recognition/detection/objects" + if self.args.detected_object + else "/perception/object_recognition/tracking/objects" + if self.args.tracked_object + else "/perception/object_recognition/objects" ) ego_odom_topic = "/localization/kinematic_state" traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" @@ -280,9 +315,11 @@ def load_rosbag(self, rosbag2_path: str): if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("-b", "--bag", help="rosbag", default=None) - parser.add_argument("-d", "--directory", help="directory of rosbags", default=None) parser.add_argument( - "-p", "--predicted-object", help="publish predicted object", action="store_true" + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" ) args = parser.parse_args() diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index ae8451b1fc696..3ce69aa265090 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -332,7 +332,6 @@ def plotTrajectoryVelocity(self, data): self.update_behavior_path_planner_path = False if len(y) != 0: self.max_vel = max(10.0, np.max(y)) - self.min_vel = np.min(y) if self.update_behavior_velocity_planner_path: x = self.CalcArcLengthPath(behavior_velocity_planner_path) @@ -396,6 +395,9 @@ def plotTrajectoryVelocity(self, data): y = [self.velocity_limit, self.velocity_limit] self.im12.set_data(x, y) + if len(y) != 0: + self.min_vel = np.min(y) + # change y-range self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0]) diff --git a/planning/planning_test_utils/README.md b/planning/planning_test_utils/README.md index d8447fdbf1122..63f1bf53a4954 100644 --- a/planning/planning_test_utils/README.md +++ b/planning/planning_test_utils/README.md @@ -1,17 +1,92 @@ -# Planning Test Manager +# Planning Interface Test Manager + +## Background + +In each node of the planning module, when exceptional input, such as unusual routes or significantly deviated ego-position, is given, the node may not be prepared for such input and could crash. As a result, debugging node crashes can be time-consuming. For example, if an empty trajectory is given as input and it was not anticipated during implementation, the node might crash due to the unaddressed exceptional input when changes are merged, during scenario testing or while the system is running on an actual vehicle. ## Purpose -## Inner-workings / Algorithms +The purpose is to provide a utility for implementing tests to ensure that node operates correctly when receiving exceptional input. By utilizing this utility and implementing tests for exceptional input, the purpose is to reduce bugs that are only discovered when actually running the system, by requiring measures for exceptional input before merging PRs. + +## Features + +### Confirmation of normal operation + +For the test target node, confirm that the node operates correctly and publishes the required messages for subsequent nodes. To do this, test_node publish the necessary messages and confirm that the node's output is being published. + +### Robustness confirmation for special inputs + +After confirming normal operation, ensure that the test target node does not crash when given exceptional input. To do this, provide exceptional input from the test_node and confirm that the node does not crash. + +(WIP) + +## Usage + +```cpp + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); -## Inputs / Outputs + // instantiate test_manager with PlanningInterfaceTestManager type + auto test_manager = std::make_shared(); -### Inputs + // get package directories for necessary configuration files + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto target_node_dir = + ament_index_cpp::get_package_share_directory("target_node"); -### Outputs + // set arguments to get the config file + node_options.arguments( + {"--ros-args", "--params-file", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + planning_validator_dir + "/config/planning_validator.param.yaml"}); -## Parameters + // instantiate the TargetNode with node_options + auto test_target_node = std::make_shared(node_options); -## Assumptions / Known limits + // publish the necessary topics from test_manager second argument is topic name + test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); + test_manager->publishMaxVelocity( + test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + + // set scenario_selector's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); + + // test with normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + + // make sure target_node is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with trajectory input with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + + // shutdown ROS context + rclcpp::shutdown(); +} +``` + +## Implemented tests + +| Node | Test name | exceptional input | output | Exceptional input pattern | +| -------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| motion_velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_avoidance_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | +| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | +| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | +| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | + +## Important Notes + +During test execution, when launching a node, parameters are loaded from the parameter file within each package. Therefore, when adding parameters, it is necessary to add the required parameters to the parameter file in the target node package. This is to prevent the node from being unable to launch if there are missing parameters when retrieving them from the parameter file during node launch. ## Future extensions / Unimplemented parts + +(WIP) diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index 5e37c7d35c91b..5edc7b00f94a4 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,21 @@ #ifndef PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ #define PLANNING_INTERFACE_TEST_MANAGER__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) \ + try { \ + statement; \ + SUCCEED(); \ + } catch (const std::exception & e) { \ + FAIL() << "Expected: " << #statement \ + << " doesn't throw an exception.\nActual: it throws. Error message: " << e.what() \ + << std::endl; \ + } catch (...) { \ + FAIL() << "Expected: " << #statement \ + << " doesn't throw an exception.\nActual: it throws. Error message is not available." \ + << std::endl; \ + } + #include #include #include @@ -66,7 +81,6 @@ using autoware_auto_perception_msgs::msg::TrafficSignalArray; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; @@ -91,8 +105,12 @@ class PlanningInterfaceTestManager public: PlanningInterfaceTestManager(); - void publishOdometry(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishInitialPose(rclcpp::Node::SharedPtr target_node, std::string topic_name); + void publishOdometry( + rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0); + + void publishInitialPose( + rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0); + void publishMaxVelocity(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishPointCloud(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishAcceleration(rclcpp::Node::SharedPtr target_node, std::string topic_name); @@ -101,7 +119,6 @@ class PlanningInterfaceTestManager void publishOccupancyGrid(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishCostMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishLaneDrivingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishParkingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishParkingState(rclcpp::Node::SharedPtr target_node, std::string topic_name); @@ -112,12 +129,7 @@ class PlanningInterfaceTestManager void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExternalTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishVirtualTrafficLightState(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExternalCrosswalkStates(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExternalIntersectionStates( - rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishInitialPoseData(rclcpp::Node::SharedPtr target_node, std::string topic_name); void setTrajectoryInputTopicName(std::string topic_name); void setParkingTrajectoryInputTopicName(std::string topic_name); @@ -125,6 +137,7 @@ class PlanningInterfaceTestManager void setRouteInputTopicName(std::string topic_name); void setPathInputTopicName(std::string topic_name); void setPathWithLaneIdTopicName(std::string topic_name); + void setPathTopicName(std::string topic_name); void setTrajectorySubscriber(std::string topic_name); void setScenarioSubscriber(std::string topic_name); @@ -132,6 +145,9 @@ class PlanningInterfaceTestManager void setRouteSubscriber(std::string topic_name); void setPathSubscriber(std::string topic_name); + void setInitialPoseTopicName(std::string topic_name); + void setOdometryTopicName(std::string topic_name); + void testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node); void testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node); @@ -143,6 +159,21 @@ class PlanningInterfaceTestManager void testWithNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node); void testWithAbnormalPathWithLaneId(rclcpp::Node::SharedPtr target_node); + void testWithNominalPath(rclcpp::Node::SharedPtr target_node); + void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node); + + // for invalid ego poses, contains some tests inside. + void testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); + void testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); + void testPathWithLaneIdWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); + void testTrajectoryWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); + + // ego vehicle is located far from trajectory + void testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node); + void testOffTrackFromPath(rclcpp::Node::SharedPtr target_node); + void testOffTrackFromPathWithLaneId(rclcpp::Node::SharedPtr target_node); + void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node); + int getReceivedTopicNum(); private: @@ -168,10 +199,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; - rclcpp::Publisher::SharedPtr external_traffic_signals_pub_; rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; - rclcpp::Publisher::SharedPtr external_crosswalk_states_pub_; - rclcpp::Publisher::SharedPtr external_intersection_states_pub_; // Subscriber rclcpp::Subscription::SharedPtr traj_sub_; @@ -195,12 +223,18 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr normal_path_with_lane_id_pub_; rclcpp::Publisher::SharedPtr abnormal_path_with_lane_id_pub_; - std::string input_trajectory_name_; - std::string input_parking_trajectory_name_; - std::string input_lane_driving_trajectory_name_; - std::string input_route_name_; - std::string input_path_name_; - std::string input_path_with_lane_id_name_; + // Publisher for testing(Path) + rclcpp::Publisher::SharedPtr normal_path_pub_; + rclcpp::Publisher::SharedPtr abnormal_path_pub_; + + std::string input_trajectory_name_ = ""; + std::string input_parking_trajectory_name_ = ""; + std::string input_lane_driving_trajectory_name_ = ""; + std::string input_route_name_ = ""; + std::string input_path_name_ = ""; + std::string input_path_with_lane_id_name_ = ""; + std::string input_initial_pose_name_ = ""; // for the map based pose + std::string input_odometry_name_ = ""; // Node rclcpp::Node::SharedPtr test_node_; @@ -222,8 +256,8 @@ class PlanningInterfaceTestManager void publishNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishAbNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void set_initial_state_with_transform(Odometry::SharedPtr & odometry); - TransformStamped get_transform_msg(const std::string parent_frame, const std::string child_frame); + void publishNominalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); + void publishAbnormalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); }; // class PlanningInterfaceTestManager } // namespace planning_test_utils 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_interface_test_manager/planning_interface_test_manager_utils.hpp index 16c5c2cac164f..15ff71b1e9f5f 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp @@ -41,6 +41,7 @@ #include +#include #include #include #include @@ -56,6 +57,7 @@ namespace test_utils { using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; @@ -118,18 +120,6 @@ T generateTrajectory( return traj; } -Route::Message makeNormalRoute() -{ - const double pi = 3.1415926; - const std::array start_pose{5.5, 4., 0., pi * 0.5}; - const std::array goal_pose{8.0, 26.3, 0, 0}; - Route::Message route; - route.header.frame_id = "map"; - route.start_pose = createPose(start_pose); - route.goal_pose = createPose(goal_pose); - return route; -} - LaneletSegment createLaneletSegment(int id) { LaneletPrimitive primitive; @@ -141,6 +131,51 @@ LaneletSegment createLaneletSegment(int id) return segment; } +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; +} + +HADMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}, map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + HADMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.format_version = format_version; + map_bin_msg.map_version = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +Route::Message makeNormalRoute() +{ + const std::array start_pose{5.5, 4., 0., M_PI_2}; + const std::array goal_pose{8.0, 26.3, 0, 0}; + Route::Message route; + route.header.frame_id = "map"; + route.start_pose = createPose(start_pose); + route.goal_pose = createPose(goal_pose); + return route; +} + +// this is for the test lanelet2_map.osm +// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 Route::Message makeBehaviorNormalRoute() { Route::Message route; @@ -163,11 +198,12 @@ Route::Message makeBehaviorNormalRoute() return route; } -OccupancyGrid constructCostMap(size_t width, size_t height, double resolution) +OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2) { - nav_msgs::msg::OccupancyGrid costmap_msg{}; + nav_msgs::msg::OccupancyGrid costmap_msg; // create info + costmap_msg.header.frame_id = "map"; costmap_msg.info.width = width; costmap_msg.info.height = height; costmap_msg.info.resolution = resolution; @@ -180,48 +216,78 @@ OccupancyGrid constructCostMap(size_t width, size_t height, double resolution) return costmap_msg; } -void spinSomeNodes( - rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, - const int repeat_count = 1) +HADMapBin makeMapBinMsg() { - for (int i = 0; i < repeat_count; i++) { - rclcpp::spin_some(test_node); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(target_node); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; + double center_line_resolution = 5.0; + // load map from file + const auto map = loadMap(lanelet2_path); + if (!map) { + return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; } -lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +Odometry makeOdometry(const double shift = 0.0) { - lanelet::ErrorMessages errors{}; - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); - } - return nullptr; + Odometry odometry; + const std::array start_pose{0.0, shift, 0.0, 0.0}; + odometry.pose.pose = createPose(start_pose); + odometry.header.frame_id = "map"; + return odometry; } -HADMapBin createMapBinMsg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +Odometry makeInitialPose(const double shift = 0.0) { - std::string format_version{}, map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); + Odometry current_odometry; + const auto yaw = 0.9724497591854532; + const auto shift_x = shift * std::sin(yaw); + const auto shift_y = shift * std::cos(yaw); + const std::array start_pose{ + 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; + current_odometry.pose.pose = test_utils::createPose(start_pose); + current_odometry.header.frame_id = "map"; + return current_odometry; +} - HADMapBin map_bin_msg; - map_bin_msg.header.stamp = now; - map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; - lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string frame_id = "", std::string child_frame_id = "") +{ + TFMessage tf_msg; + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = 0.; + quaternion.y = 0.; + quaternion.z = 0.23311256049418302; + quaternion.w = 0.9724497591854532; + + TransformStamped tf; + tf.header.stamp = target_node->get_clock()->now(); + tf.header.frame_id = frame_id; + tf.child_frame_id = child_frame_id; + tf.transform.translation.x = 3722.16015625; + tf.transform.translation.y = 73723.515625; + tf.transform.translation.z = 0; + tf.transform.rotation = quaternion; + + tf_msg.transforms.emplace_back(std::move(tf)); + return tf_msg; +} - return map_bin_msg; +Scenario makeScenarioMsg(const std::string scenario) +{ + Scenario scenario_msg; + scenario_msg.current_scenario = scenario; + scenario_msg.activating_scenarios = {scenario}; + return scenario_msg; } template @@ -252,85 +318,6 @@ void setPublisher( createPublisherWithQoS(test_node, topic_name, publisher); } -template -T generateObject(rclcpp::Node::SharedPtr target_node) -{ - if constexpr (std::is_same_v) { - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; - double center_line_resolution = 5.0; - // load map from file - const auto map = loadMap(lanelet2_path); - if (!map) { - return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; - } - - // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); - - // create map bin msg - const auto map_bin_msg = createMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now()); - return map_bin_msg; - } else if constexpr (std::is_same_v) { - std::shared_ptr trajectory = std::make_shared(); - trajectory->header.stamp = target_node->now(); - return *trajectory; - } else if constexpr (std::is_same_v) { - std::shared_ptr current_odometry = std::make_shared(); - const double pi = 3.1415926; - const std::array start_pose{5.5, 4., pi * 0.5}; - current_odometry->pose.pose = createPose(start_pose); - current_odometry->header.frame_id = "map"; - return *current_odometry; - } else if constexpr (std::is_same_v) { - auto costmap_msg = constructCostMap(150, 150, 0.2); - costmap_msg.header.frame_id = "map"; - return costmap_msg; - } else if constexpr (std::is_same_v) { - return makeNormalRoute(); - } else if constexpr (std::is_same_v) { - TransformStamped tf; - tf.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); - tf.header.frame_id = "base_link"; - tf.child_frame_id = "map"; - - TFMessage tf_msg{}; - tf_msg.transforms.emplace_back(std::move(tf)); - return tf_msg; - } else if constexpr (std::is_same_v) { - PointCloud2 point_cloud_msg{}; - point_cloud_msg.header.frame_id = "base_link"; - return point_cloud_msg; - } else { - return T{}; - } -} - -template -void publishData( - rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, - typename rclcpp::Publisher::SharedPtr publisher) -{ - setPublisher(test_node, topic_name, publisher); - T object = generateObject(target_node); - publisher->publish(object); - spinSomeNodes(test_node, target_node); -} - -void publishScenarioData( - rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, - rclcpp::Publisher::SharedPtr publisher, const std::string scenario) -{ - auto scenario_msg = std::make_shared(); - scenario_msg->current_scenario = scenario; - scenario_msg->activating_scenarios = {scenario}; - setPublisher(test_node, topic_name, publisher); - publisher->publish(*scenario_msg); - - spinSomeNodes(test_node, target_node); -} - template void createSubscription( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -353,6 +340,43 @@ void setSubscriber( test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber); } +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, + const int repeat_count = 1) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); + for (int i = 0; i < repeat_count; i++) { + executor.spin_some(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + +template +void publishToTargetNode( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, + typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) +{ + if (topic_name.empty()) { + int status; + char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); + if (status == 0) { + throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); + } else { + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); + } + } + + test_utils::setPublisher(test_node, topic_name, publisher); + publisher->publish(data); + + if (target_node->count_subscribers(topic_name) == 0) { + throw std::runtime_error("No subscriber for " + topic_name); + } + test_utils::spinSomeNodes(test_node, target_node, repeat_count); +} + void updateNodeOptions( rclcpp::NodeOptions & node_options, const std::vector & params_files) { @@ -368,6 +392,19 @@ void updateNodeOptions( node_options.arguments(std::vector{arguments.begin(), arguments.end()}); } +Path toPath(const PathWithLaneId & input) +{ + Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + PathWithLaneId loadPathWithLaneIdInYaml() { const auto planning_test_utils_dir = diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 545ed510c4df0..c631603cf71ba 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -3,7 +3,7 @@ planning_test_utils 0.1.0 - ROS2 node for testing interface of the nodes in planning module + ROS 2 node for testing interface of the nodes in planning module Kyoichi Sugahara Takamasa Horibe Apache License 2.0 @@ -11,8 +11,7 @@ Kyoichi Sugahara ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_mapping_msgs @@ -35,7 +34,6 @@ unique_identifier_msgs yaml_cpp_vendor - ament_cmake_gtest ament_cmake_ros ament_lint_auto autoware_lint_common 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 7ca551df265fb..a0b887fc4d089 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -26,189 +26,157 @@ PlanningInterfaceTestManager::PlanningInterfaceTestManager() } void PlanningInterfaceTestManager::publishOdometry( - rclcpp::Node::SharedPtr target_node, std::string topic_name) + rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift) { - test_utils::publishData(test_node_, target_node, topic_name, odom_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, odom_pub_, test_utils::makeOdometry(shift)); } void PlanningInterfaceTestManager::publishMaxVelocity( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, max_velocity_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, max_velocity_pub_, VelocityLimit{}); } void PlanningInterfaceTestManager::publishPointCloud( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, point_cloud_pub_); + PointCloud2 point_cloud_msg{}; + point_cloud_msg.header.frame_id = "base_link"; + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, point_cloud_pub_, point_cloud_msg); } void PlanningInterfaceTestManager::publishAcceleration( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData( - test_node_, target_node, topic_name, acceleration_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, acceleration_pub_, AccelWithCovarianceStamped{}); } void PlanningInterfaceTestManager::publishPredictedObjects( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData( - test_node_, target_node, topic_name, predicted_objects_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{}); } void PlanningInterfaceTestManager::publishExpandStopRange( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData( - test_node_, target_node, topic_name, expand_stop_range_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, expand_stop_range_pub_, ExpandStopRange{}); } void PlanningInterfaceTestManager::publishOccupancyGrid( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, occupancy_grid_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, occupancy_grid_pub_, test_utils::makeCostMapMsg()); } void PlanningInterfaceTestManager::publishCostMap( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, cost_map_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, cost_map_pub_, test_utils::makeCostMapMsg()); } void PlanningInterfaceTestManager::publishMap( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, map_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, map_pub_, test_utils::makeMapBinMsg()); } void PlanningInterfaceTestManager::publishLaneDrivingScenario( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishScenarioData( - test_node_, target_node, topic_name, lane_driving_scenario_pub_, Scenario::LANEDRIVING); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, lane_driving_scenario_pub_, + test_utils::makeScenarioMsg(Scenario::LANEDRIVING)); } void PlanningInterfaceTestManager::publishParkingScenario( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishScenarioData( - test_node_, target_node, topic_name, parking_scenario_pub_, Scenario::PARKING); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, parking_scenario_pub_, + test_utils::makeScenarioMsg(Scenario::PARKING)); } void PlanningInterfaceTestManager::publishInitialPose( - rclcpp::Node::SharedPtr target_node, std::string topic_name) + rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift) { - publishInitialPoseData(target_node, topic_name); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, initial_pose_pub_, test_utils::makeInitialPose(shift)); } void PlanningInterfaceTestManager::publishParkingState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData( - test_node_, target_node, topic_name, parking_state_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, parking_state_pub_, std_msgs::msg::Bool{}); } void PlanningInterfaceTestManager::publishTrajectory( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, trajectory_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, trajectory_pub_, Trajectory{}); } void PlanningInterfaceTestManager::publishRoute( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, route_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, route_pub_, test_utils::makeNormalRoute()); } void PlanningInterfaceTestManager::publishTF( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, TF_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, TF_pub_, + test_utils::makeTFMsg(target_node, "base_link", "map")); } void PlanningInterfaceTestManager::publishLateralOffset( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData(test_node_, target_node, topic_name, lateral_offset_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, lateral_offset_pub_, LateralOffset{}); } void PlanningInterfaceTestManager::publishOperationModeState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData( - test_node_, target_node, topic_name, operation_mode_state_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, operation_mode_state_pub_, OperationModeState{}); } void PlanningInterfaceTestManager::publishTrafficSignals( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::publishData( - test_node_, target_node, topic_name, traffic_signals_pub_); -} -void PlanningInterfaceTestManager::publishExternalTrafficSignals( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishData( - test_node_, target_node, topic_name, external_traffic_signals_pub_); -} -void PlanningInterfaceTestManager::publishVirtualTrafficLightState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishData( - test_node_, target_node, topic_name, virtual_traffic_light_states_pub_); -} -void PlanningInterfaceTestManager::publishExternalCrosswalkStates( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishData( - test_node_, target_node, topic_name, external_crosswalk_states_pub_); -} -void PlanningInterfaceTestManager::publishExternalIntersectionStates( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishData( - test_node_, target_node, topic_name, external_intersection_states_pub_); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, traffic_signals_pub_, TrafficSignalArray{}); } -void PlanningInterfaceTestManager::publishInitialPoseData( +void PlanningInterfaceTestManager::publishVirtualTrafficLightState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - std::shared_ptr current_odometry = std::make_shared(); - const std::array start_pose{ - 3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}; - current_odometry->pose.pose = test_utils::createPose(start_pose); - current_odometry->header.frame_id = "map"; - - test_utils::setPublisher(test_node_, topic_name, initial_pose_pub_); - initial_pose_pub_->publish(*current_odometry); - test_utils::spinSomeNodes(test_node_, target_node); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, virtual_traffic_light_states_pub_, + VirtualTrafficLightStateArray{}); } void PlanningInterfaceTestManager::publishInitialPoseTF( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = 0.; - quaternion.y = 0.; - quaternion.z = 0.23311256049418302; - quaternion.w = 0.9724497591854532; - - TransformStamped tf; - tf.header.stamp = target_node->get_clock()->now(); - tf.header.frame_id = "odom"; - tf.child_frame_id = "base_link"; - tf.transform.translation.x = 3722.16015625; - tf.transform.translation.y = 73723.515625; - tf.transform.translation.z = 0; - tf.transform.rotation = quaternion; - - tf2_msgs::msg::TFMessage tf_msg{}; - tf_msg.transforms.emplace_back(std::move(tf)); - - test_utils::setPublisher(test_node_, topic_name, initial_pose_tf_pub_); - initial_pose_tf_pub_->publish(tf_msg); - test_utils::spinSomeNodes(test_node_, target_node); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, initial_pose_tf_pub_, + test_utils::makeTFMsg(target_node, "odom", "base_link")); } void PlanningInterfaceTestManager::setTrajectoryInputTopicName(std::string topic_name) @@ -231,66 +199,91 @@ void PlanningInterfaceTestManager::setRouteInputTopicName(std::string topic_name input_route_name_ = topic_name; } +void PlanningInterfaceTestManager::setPathInputTopicName(std::string topic_name) +{ + input_path_name_ = topic_name; +} + void PlanningInterfaceTestManager::setPathWithLaneIdTopicName(std::string topic_name) { input_path_with_lane_id_name_ = topic_name; } +void PlanningInterfaceTestManager::setInitialPoseTopicName(std::string topic_name) +{ + input_initial_pose_name_ = topic_name; +} + +void PlanningInterfaceTestManager::setOdometryTopicName(std::string topic_name) +{ + input_odometry_name_ = topic_name; +} + void PlanningInterfaceTestManager::publishNominalTrajectory( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::setPublisher(test_node_, topic_name, normal_trajectory_pub_); - normal_trajectory_pub_->publish(test_utils::generateTrajectory(10, 1.0)); - test_utils::spinSomeNodes(test_node_, target_node); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_trajectory_pub_, + test_utils::generateTrajectory(10, 1.0), 5); } void PlanningInterfaceTestManager::publishAbnormalTrajectory( rclcpp::Node::SharedPtr target_node, const Trajectory & abnormal_trajectory) { - test_utils::setPublisher(test_node_, input_trajectory_name_, abnormal_trajectory_pub_); - abnormal_trajectory_pub_->publish(abnormal_trajectory); - test_utils::spinSomeNodes(test_node_, target_node); + test_utils::publishToTargetNode( + test_node_, target_node, input_trajectory_name_, abnormal_trajectory_pub_, abnormal_trajectory); } void PlanningInterfaceTestManager::publishNominalRoute( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::setPublisher(test_node_, topic_name, normal_route_pub_); - normal_route_pub_->publish(test_utils::makeNormalRoute()); - test_utils::spinSomeNodes(test_node_, target_node); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_route_pub_, test_utils::makeNormalRoute(), 5); } void PlanningInterfaceTestManager::publishBehaviorNominalRoute( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::setPublisher(test_node_, topic_name, behavior_normal_route_pub_); - behavior_normal_route_pub_->publish(test_utils::makeBehaviorNormalRoute()); - test_utils::spinSomeNodes(test_node_, target_node); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, behavior_normal_route_pub_, + test_utils::makeBehaviorNormalRoute(), 5); } void PlanningInterfaceTestManager::publishAbnormalRoute( rclcpp::Node::SharedPtr target_node, const LaneletRoute & abnormal_route) { - test_utils::setPublisher(test_node_, input_route_name_, abnormal_route_pub_); - abnormal_route_pub_->publish(abnormal_route); - test_utils::spinSomeNodes(test_node_, target_node, 5); + test_utils::publishToTargetNode( + test_node_, target_node, input_route_name_, abnormal_route_pub_, abnormal_route, 5); } void PlanningInterfaceTestManager::publishNominalPathWithLaneId( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::setPublisher(test_node_, topic_name, normal_path_with_lane_id_pub_); - normal_path_with_lane_id_pub_->publish(test_utils::loadPathWithLaneIdInYaml()); - - test_utils::spinSomeNodes(test_node_, target_node, 5); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_path_with_lane_id_pub_, + test_utils::loadPathWithLaneIdInYaml(), 5); } void PlanningInterfaceTestManager::publishAbNominalPathWithLaneId( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - test_utils::setPublisher(test_node_, topic_name, abnormal_path_with_lane_id_pub_); - normal_path_with_lane_id_pub_->publish(PathWithLaneId{}); - test_utils::spinSomeNodes(test_node_, target_node, 5); + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, abnormal_path_with_lane_id_pub_, PathWithLaneId{}, 5); +} + +void PlanningInterfaceTestManager::publishNominalPath( + rclcpp::Node::SharedPtr target_node, std::string topic_name) +{ + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_path_pub_, + test_utils::toPath(test_utils::loadPathWithLaneIdInYaml()), 5); +} + +void PlanningInterfaceTestManager::publishAbnormalPath( + rclcpp::Node::SharedPtr target_node, std::string topic_name) +{ + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, abnormal_path_pub_, Path{}, 5); } void PlanningInterfaceTestManager::setTrajectorySubscriber(std::string topic_name) @@ -321,49 +314,40 @@ void PlanningInterfaceTestManager::setPathSubscriber(std::string topic_name) void PlanningInterfaceTestManager::testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node) { publishNominalTrajectory(target_node, input_trajectory_name_); - test_utils::spinSomeNodes(test_node_, target_node, 2); } // check to see if target node is dead. void PlanningInterfaceTestManager::testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node) { - ASSERT_NO_THROW(publishAbnormalTrajectory(target_node, Trajectory{})); - ASSERT_NO_THROW( - publishAbnormalTrajectory(target_node, test_utils::generateTrajectory(1, 0.0))); - ASSERT_NO_THROW(publishAbnormalTrajectory( - target_node, test_utils::generateTrajectory(10, 0.0, 0.0, 0.0, 0.0, 1))); + publishAbnormalTrajectory(target_node, Trajectory{}); + publishAbnormalTrajectory(target_node, test_utils::generateTrajectory(1, 0.0)); + publishAbnormalTrajectory( + target_node, test_utils::generateTrajectory(10, 0.0, 0.0, 0.0, 0.0, 1)); } // test for normal working void PlanningInterfaceTestManager::testWithNominalRoute(rclcpp::Node::SharedPtr target_node) { publishNominalRoute(target_node, input_route_name_); - test_utils::spinSomeNodes(test_node_, target_node, 5); } // test for normal working void PlanningInterfaceTestManager::testWithBehaviorNominalRoute(rclcpp::Node::SharedPtr target_node) { publishBehaviorNominalRoute(target_node, input_route_name_); - test_utils::spinSomeNodes(test_node_, target_node, 5); } // check to see if target node is dead. void PlanningInterfaceTestManager::testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node) { - ASSERT_NO_THROW(publishAbnormalRoute(target_node, LaneletRoute{})); - test_utils::spinSomeNodes(test_node_, target_node, 5); + publishAbnormalRoute(target_node, LaneletRoute{}); } // test for normal working void PlanningInterfaceTestManager::testWithNominalPathWithLaneId( rclcpp::Node::SharedPtr target_node) { - std::cerr << __FILE__ << ": " << __LINE__ << std::endl; publishNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); - std::cerr << __FILE__ << ": " << __LINE__ << std::endl; - test_utils::spinSomeNodes(test_node_, target_node, 5); - std::cerr << __FILE__ << ": " << __LINE__ << std::endl; } // check to see if target node is dead. @@ -371,7 +355,79 @@ void PlanningInterfaceTestManager::testWithAbnormalPathWithLaneId( rclcpp::Node::SharedPtr target_node) { publishAbNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); - test_utils::spinSomeNodes(test_node_, target_node, 5); +} + +// put all abnormal ego pose related tests in this functions to run all tests with one line code +void PlanningInterfaceTestManager::testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node) +{ + testOffTrackFromRoute(target_node); +} + +// put all abnormal ego pose related tests in this functions to run all tests with one line code +void PlanningInterfaceTestManager::testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node) +{ + testOffTrackFromPath(target_node); +} + +// put all abnormal ego pose related tests in this functions to run all tests with one line code +void PlanningInterfaceTestManager::testPathWithLaneIdWithInvalidEgoPose( + rclcpp::Node::SharedPtr target_node) +{ + testOffTrackFromPathWithLaneId(target_node); +} + +// put all abnormal ego pose related tests in this functions to run all tests with one line code +void PlanningInterfaceTestManager::testTrajectoryWithInvalidEgoPose( + rclcpp::Node::SharedPtr target_node) +{ + testOffTrackFromTrajectory(target_node); +} + +void PlanningInterfaceTestManager::testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node) +{ + publishBehaviorNominalRoute(target_node, input_route_name_); + + const std::vector deviation_from_route = {0.0, 1.0, 10.0, 100.0}; + for (const auto & deviation : deviation_from_route) { + publishInitialPose(target_node, input_initial_pose_name_, deviation); + } +} + +void PlanningInterfaceTestManager::testOffTrackFromPath(rclcpp::Node::SharedPtr target_node) +{ + // write me + (void)target_node; +} + +void PlanningInterfaceTestManager::testOffTrackFromPathWithLaneId( + rclcpp::Node::SharedPtr target_node) +{ + publishNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); + + const std::vector deviation_from_path = {0.0, 1.0, 10.0, 100.0}; + for (const auto & deviation : deviation_from_path) { + publishOdometry(target_node, input_odometry_name_, deviation); + } +} + +void PlanningInterfaceTestManager::testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node) +{ + publishNominalTrajectory(target_node, input_trajectory_name_); + + const std::vector deviation_from_traj = {0.0, 1.0, 10.0, 100.0}; + for (const auto & deviation : deviation_from_traj) { + publishOdometry(target_node, input_odometry_name_, deviation); + } +} + +void PlanningInterfaceTestManager::testWithNominalPath(rclcpp::Node::SharedPtr target_node) +{ + publishNominalPath(target_node, input_path_name_); +} + +void PlanningInterfaceTestManager::testWithAbnormalPath(rclcpp::Node::SharedPtr target_node) +{ + publishAbnormalPath(target_node, input_path_name_); } int PlanningInterfaceTestManager::getReceivedTopicNum() diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 68c640346c75a..685c2823796cc 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -12,7 +12,7 @@ Yutaka Shimizu ament_cmake_auto - autoware_cmake + autoware_cmake rosidl_default_generators autoware_auto_planning_msgs 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 33eeb7be97120..95619e749a9f9 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,38 +21,72 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +using planning_test_utils::PlanningInterfaceTestManager; +using planning_validator::PlanningValidator; + +std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - auto node_options = rclcpp::NodeOptions{}; + // set subscriber with topic name: planning_validator → test_node_ + test_manager->setTrajectorySubscriber("planning_validator/output/trajectory"); + + // set planning_validator's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory"); + + test_manager->setOdometryTopicName("planning_validator/input/kinematics"); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto planning_validator_dir = ament_index_cpp::get_package_share_directory("planning_validator"); - node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", planning_validator_dir + "/config/planning_validator.param.yaml"}); + return std::make_shared(node_options); +} - auto test_target_node = std::make_shared(node_options); - +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "planning_validator/input/kinematics"); +} - // set subscriber with topic name: planning_validator → test_node_ - test_manager->setTrajectorySubscriber("planning_validator/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // set planning_validator's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory"); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - test_manager->testWithAbnormalTrajectory(test_target_node); + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 514c147f26d8f..a83bae2489eb8 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -77,6 +77,7 @@ class RouteHandler lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const; Header getRouteHeader() const; UUID getRouteUuid() const; + bool isAllowedGoalModification() const; // for routing graph bool isMapMsgReady() const; @@ -104,6 +105,8 @@ class RouteHandler // for lanelet bool getPreviousLaneletsWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; + bool getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const; @@ -302,11 +305,13 @@ class RouteHandler lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const; lanelet::ConstLanelets getLaneletSequence( const lanelet::ConstLanelet & lanelet, const Pose & current_pose, - const double backward_distance, const double forward_distance) const; + const double backward_distance, const double forward_distance, + const bool only_route_lanes = true) const; lanelet::ConstLanelets getLaneletSequence( const lanelet::ConstLanelet & lanelet, const double backward_distance = std::numeric_limits::max(), - const double forward_distance = std::numeric_limits::max()) const; + const double forward_distance = std::numeric_limits::max(), + const bool only_route_lanes = true) const; lanelet::ConstLanelets getShoulderLaneletSequence( const lanelet::ConstLanelet & lanelet, const Pose & pose, const double backward_distance = std::numeric_limits::max(), @@ -317,6 +322,7 @@ class RouteHandler lanelet::routing::RelationType getRelation( const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const; lanelet::ConstLanelets getShoulderLanelets() const; + bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; // for path PathWithLaneId getCenterLinePath( @@ -337,7 +343,12 @@ class RouteHandler const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, lanelet::ConstLanelet * target_lanelet); double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const; + bool getLeftShoulderLanelet( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const; + bool getRightShoulderLanelet( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; + bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const; private: // MUST @@ -371,8 +382,6 @@ class RouteHandler bool isBijectiveConnection( const lanelet::ConstLanelets & lanelet_section1, const lanelet::ConstLanelets & lanelet_section2) const; - bool getNextLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; bool getPreviousLaneletWithinRouteExceptGoal( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const; bool getNextLaneletWithinRouteExceptStart( @@ -384,10 +393,12 @@ class RouteHandler lanelet::ConstLanelets getRouteLanelets() const; lanelet::ConstLanelets getLaneletSequenceUpTo( const lanelet::ConstLanelet & lanelet, - const double min_length = std::numeric_limits::max()) const; + const double min_length = std::numeric_limits::max(), + const bool only_route_lanes = true) const; lanelet::ConstLanelets getLaneletSequenceAfter( const lanelet::ConstLanelet & lanelet, - const double min_length = std::numeric_limits::max()) const; + const double min_length = std::numeric_limits::max(), + const bool only_route_lanes = true) const; bool getFollowingShoulderLanelet( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * following_lanelet) const; lanelet::ConstLanelets getShoulderLaneletSequenceAfter( diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index fb863d2e825b3..fc38d02845796 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -13,8 +13,7 @@ Fumiya Watanabe ament_cmake_auto - - autoware_cmake + autoware_cmake ament_lint_auto autoware_lint_common diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 014b0973bd1e2..d9cdfd64129d8 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -368,6 +368,15 @@ UUID RouteHandler::getRouteUuid() const return route_ptr_->uuid; } +bool RouteHandler::isAllowedGoalModification() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); + return false; + } + return route_ptr_->allow_modification; +} + std::vector RouteHandler::getLanesBeforePose( const geometry_msgs::msg::Pose & pose, const double length) const { @@ -475,10 +484,10 @@ lanelet::ConstLanelets RouteHandler::getLaneChangeableNeighbors( } lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( - const lanelet::ConstLanelet & lanelet, const double min_length) const + const lanelet::ConstLanelet & lanelet, const double min_length, const bool only_route_lanes) const { lanelet::ConstLanelets lanelet_sequence_forward; - if (!exists(route_lanelets_, lanelet)) { + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { return lanelet_sequence_forward; } @@ -503,10 +512,10 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( } lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( - const lanelet::ConstLanelet & lanelet, const double min_length) const + const lanelet::ConstLanelet & lanelet, const double min_length, const bool only_route_lanes) const { lanelet::ConstLanelets lanelet_sequence_backward; - if (!exists(route_lanelets_, lanelet)) { + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { return lanelet_sequence_backward; } @@ -560,7 +569,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelets RouteHandler::getLaneletSequence( const lanelet::ConstLanelet & lanelet, const double backward_distance, - const double forward_distance) const + const double forward_distance, const bool only_route_lanes) const { Pose current_pose{}; current_pose.orientation.w = 1; @@ -569,16 +578,16 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( } lanelet::ConstLanelets lanelet_sequence; - if (!exists(route_lanelets_, lanelet)) { + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { return lanelet_sequence; } lanelet::ConstLanelets lanelet_sequence_forward = - getLaneletSequenceAfter(lanelet, forward_distance); + getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { - return getLaneletSequenceUpTo(lanelet, backward_distance); + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); } return lanelet::ConstLanelets{}; }); @@ -600,19 +609,19 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( lanelet::ConstLanelets RouteHandler::getLaneletSequence( const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance, - const double forward_distance) const + const double forward_distance, const bool only_route_lanes) const { lanelet::ConstLanelets lanelet_sequence; - if (!exists(route_lanelets_, lanelet)) { + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { return lanelet_sequence; } lanelet::ConstLanelets lanelet_sequence_forward = - getLaneletSequenceAfter(lanelet, forward_distance); + getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { - return getLaneletSequenceUpTo(lanelet, backward_distance); + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); } return lanelet::ConstLanelets{}; }); @@ -644,6 +653,30 @@ bool RouteHandler::getFollowingShoulderLanelet( return false; } +bool RouteHandler::getLeftShoulderLanelet( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const +{ + for (const auto & shoulder_lanelet : shoulder_lanelets_) { + if (lanelet::geometry::leftOf(shoulder_lanelet, lanelet)) { + *left_lanelet = shoulder_lanelet; + return true; + } + } + return false; +} + +bool RouteHandler::getRightShoulderLanelet( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const +{ + for (const auto & shoulder_lanelet : shoulder_lanelets_) { + if (lanelet::geometry::rightOf(shoulder_lanelet, lanelet)) { + *right_lanelet = shoulder_lanelet; + return true; + } + } + return false; +} + lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter( const lanelet::ConstLanelet & lanelet, const double min_length) const { @@ -1391,6 +1424,11 @@ std::vector RouteHandler::getLateralIntervalsToPreferredLane( return {}; } +bool RouteHandler::isPreferredLane(const lanelet::ConstLanelet & lanelet) const +{ + return exists(preferred_lanelets_, lanelet); +} + bool RouteHandler::isInPreferredLane(const PoseStamped & pose) const { lanelet::ConstLanelet lanelet; @@ -1399,6 +1437,7 @@ bool RouteHandler::isInPreferredLane(const PoseStamped & pose) const } return exists(preferred_lanelets_, lanelet); } + bool RouteHandler::isInTargetLane( const PoseStamped & pose, const lanelet::ConstLanelets & target) const { @@ -1683,6 +1722,11 @@ lanelet::ConstLanelets RouteHandler::getShoulderLanelets() const return shoulder_lanelets_; } +bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::contains(shoulder_lanelets_, lanelet); +} + lanelet::ConstLanelets RouteHandler::getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const { diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml index afda1b04f9add..06ec042c30a7a 100644 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml @@ -13,8 +13,9 @@ - "lane_change_right" - "avoidance_left" - "avoidance_right" - - "pull_over" + - "goal_planner" - "pull_out" + - "intersection_occlusion" default_enable_list: - "blind_spot" @@ -29,5 +30,6 @@ - "lane_change_right" - "avoidance_left" - "avoidance_right" - - "pull_over" + - "goal_planner" - "pull_out" + - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/package.xml b/planning/rtc_auto_mode_manager/package.xml index dc6d452df0649..e9c16aac1bb91 100644 --- a/planning/rtc_auto_mode_manager/package.xml +++ b/planning/rtc_auto_mode_manager/package.xml @@ -13,8 +13,7 @@ Fumiya Watanabe ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp rclcpp_components diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index 028876fef4a6b..33970700991c7 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -43,14 +43,20 @@ Module getModuleType(const std::string & module_name) module.type = Module::LANE_CHANGE_LEFT; } else if (module_name == "lane_change_right") { module.type = Module::LANE_CHANGE_RIGHT; + } else if (module_name == "avoidance_by_lane_change_left") { + module.type = Module::AVOIDANCE_BY_LC_LEFT; + } else if (module_name == "avoidance_by_lane_change_right") { + module.type = Module::AVOIDANCE_BY_LC_RIGHT; } else if (module_name == "avoidance_left") { module.type = Module::AVOIDANCE_LEFT; } else if (module_name == "avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "pull_over") { - module.type = Module::PULL_OVER; + } else if (module_name == "goal_planner") { + module.type = Module::GOAL_PLANNER; } else if (module_name == "pull_out") { module.type = Module::PULL_OUT; + } else if (module_name == "intersection_occlusion") { + module.type = Module::INTERSECTION_OCCLUSION; } else { module.type = Module::NONE; } diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index af1f3da4331bd..a06a023671690 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -64,10 +64,12 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_LEFT; } else if (module_name == "avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "pull_over") { - module.type = Module::PULL_OVER; + } else if (module_name == "goal_planner") { + module.type = Module::GOAL_PLANNER; } else if (module_name == "pull_out") { module.type = Module::PULL_OUT; + } else if (module_name == "intersection_occlusion") { + module.type = Module::INTERSECTION_OCCLUSION; } return module; } diff --git a/planning/rtc_replayer/package.xml b/planning/rtc_replayer/package.xml index 9101a1e86633a..17c5a760173d6 100644 --- a/planning/rtc_replayer/package.xml +++ b/planning/rtc_replayer/package.xml @@ -13,8 +13,7 @@ Fumiya Watanabe ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp rclcpp_components diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index 08f037fbfc390..ebc7171fa209b 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -47,8 +47,8 @@ std::string getModuleName(const uint8_t module_type) case Module::AVOIDANCE_RIGHT: { return "avoidance_right"; } - case Module::PULL_OVER: { - return "pull_over"; + case Module::GOAL_PLANNER: { + return "goal_planner"; } case Module::PULL_OUT: { return "pull_out"; diff --git a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt b/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt new file mode 100644 index 0000000000000..76d0fc99843f1 --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.5) +project(bezier_sampler) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(bezier_sampler SHARED + src/bezier.cpp + src/bezier_sampling.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/planning/sampling_based_planner/bezier_sampler/README.md b/planning/sampling_based_planner/bezier_sampler/README.md new file mode 100644 index 0000000000000..654193006fad3 --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/README.md @@ -0,0 +1,3 @@ +# Bézier sampler + +Implementation of bézier curves and their generation following the sampling strategy from diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp new file mode 100644 index 0000000000000..89d91bd3110f5 --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp @@ -0,0 +1,75 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEZIER_SAMPLER__BEZIER_HPP_ +#define BEZIER_SAMPLER__BEZIER_HPP_ + +#include + +#include + +namespace bezier_sampler +{ + +// Coefficients for matrix calculation of the quintic Bézier curve. +const Eigen::Matrix quintic_bezier_coefficients( + (Eigen::Matrix() << 1, 0, 0, 0, 0, 0, -5, 5, 0, 0, 0, 0, 10, -20, 10, 0, 0, 0, -10, + 30, -30, 10, 0, 0, 5, -20, 30, -20, 5, 0, -1, 5, -10, 10, -5, 1) + .finished()); +const Eigen::Matrix quintic_bezier_velocity_coefficients( + (Eigen::Matrix() << quintic_bezier_coefficients.row(1) * 1, + quintic_bezier_coefficients.row(2) * 2, quintic_bezier_coefficients.row(3) * 3, + quintic_bezier_coefficients.row(4) * 4, quintic_bezier_coefficients.row(5) * 5) + .finished()); +const Eigen::Matrix quintic_bezier_acceleration_coefficients( + (Eigen::Matrix() << quintic_bezier_velocity_coefficients.row(1) * 1, + quintic_bezier_velocity_coefficients.row(2) * 2, quintic_bezier_velocity_coefficients.row(3) * 3, + quintic_bezier_velocity_coefficients.row(4) * 4) + .finished()); + +/// @brief Quintic Bezier curve +class Bezier +{ + Eigen::Matrix control_points_; + +public: + /// @brief constructor from a matrix + explicit Bezier(Eigen::Matrix control_points); + /// @brief constructor from a set of control points + explicit Bezier(const std::vector & control_points); + /// @brief return the control points + [[nodiscard]] const Eigen::Matrix & getControlPoints() const; + /// @brief return the curve in cartersian frame with the desired resolution + [[nodiscard]] std::vector cartesian(const double resolution) const; + /// @brief return the curve in cartersian frame with the desired number of points + [[nodiscard]] std::vector cartesian(const int nb_points) const; + /// @brief return the curve in cartersian frame (including angle) with the desired number of + /// points + [[nodiscard]] std::vector cartesianWithHeading(const int nb_points) const; + /// @brief calculate the curve value for the given parameter t + [[nodiscard]] Eigen::Vector2d value(const double t) const; + /// @brief calculate the curve value for the given parameter t (using matrix formulation) + [[nodiscard]] Eigen::Vector2d valueM(const double t) const; + /// @brief calculate the velocity (1st derivative) for the given parameter t + [[nodiscard]] Eigen::Vector2d velocity(const double t) const; + /// @brief calculate the acceleration (2nd derivative) for the given parameter t + [[nodiscard]] Eigen::Vector2d acceleration(const double t) const; + /// @breif return the heading (in radians) of the tangent for the given parameter t + [[nodiscard]] double heading(const double t) const; + /// @brief calculate the curvature for the given parameter t + [[nodiscard]] double curvature(const double t) const; +}; +} // namespace bezier_sampler + +#endif // BEZIER_SAMPLER__BEZIER_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp new file mode 100644 index 0000000000000..9dea7d34742dc --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ + +#include +#include +#include + +#include + +namespace bezier_sampler +{ +struct SamplingParameters +{ + int nb_t; // Number of samples of normalized tangent vector magnitude + double mt_min; // Minimum normalized tangent vector magnitude + double mt_max; // Maximum normalized tangent vector magnitude + int nb_k; // Number of samples of normalized curvature vector magnitude + double mk_min; // Minimum normalized curvature vector magnitude + double mk_max; // Minimum normalized curvature vector magnitude +}; +/// @brief sample Bezier curves given an initial and final state and sampling parameters +/// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated +/// Driving in Urban Environments +std::vector sample( + const sampler_common::State & initial, const sampler_common::State & final, + const SamplingParameters & params); +/// @brief generate a Bezier curve for the given states, velocities, and accelerations +Bezier generate( + const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, + const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration, + const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration); +} // namespace bezier_sampler + +#endif // BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/package.xml b/planning/sampling_based_planner/bezier_sampler/package.xml new file mode 100644 index 0000000000000..efe65e830871a --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/package.xml @@ -0,0 +1,24 @@ + + + bezier_sampler + 0.0.1 + Package to sample trajectories using Bézier curves + Maxime CLEMENT + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + eigen + sampler_common + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp b/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp new file mode 100644 index 0000000000000..fc4c09840f507 --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp @@ -0,0 +1,120 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace bezier_sampler +{ +Bezier::Bezier(Eigen::Matrix control_points) +: control_points_(std::move(control_points)) +{ +} +Bezier::Bezier(const std::vector & control_points) +{ + if (control_points.size() != 6) { + std::cerr << "Trying to initialize a quintic bezier curve with " << control_points.size() + << " (!= 6) control points." << std::endl; + } else { + control_points_ << control_points[0], control_points[1], control_points[2], control_points[3], + control_points[4], control_points[5]; + } +} + +const Eigen::Matrix & Bezier::getControlPoints() const +{ + return control_points_; +} + +Eigen::Vector2d Bezier::value(const double t) const +{ + Eigen::Vector2d point = {0.0, 0.0}; + // sum( binomial(i in 5) * (1 - t)^(5-i) * t^i * control_points_[i] ) + point += std::pow((1 - t), 5) * control_points_.row(0); + point += 5 * std::pow((1 - t), 4) * t * control_points_.row(1); + point += 10 * std::pow((1 - t), 3) * t * t * control_points_.row(2); + point += 10 * std::pow((1 - t), 2) * t * t * t * control_points_.row(3); + point += 5 * (1 - t) * t * t * t * t * control_points_.row(4); + point += t * t * t * t * t * control_points_.row(5); + return point; +} + +Eigen::Vector2d Bezier::valueM(const double t) const +{ + Eigen::Matrix ts; + ts << 1, t, t * t, t * t * t, t * t * t * t, t * t * t * t * t; + return ts * quintic_bezier_coefficients * control_points_; +} + +std::vector Bezier::cartesian(const int nb_points) const +{ + std::vector points; + points.reserve(nb_points); + const double step = 1.0 / (nb_points - 1); + for (double t = 0.0; t <= 1.0; t += step) points.push_back(valueM(t)); + return points; +} + +std::vector Bezier::cartesian(const double resolution) const +{ + std::vector points; + points.reserve(static_cast(1 / resolution)); + for (double t = 0.0; t <= 1.0; t += resolution) points.push_back(valueM(t)); + return points; +} + +std::vector Bezier::cartesianWithHeading(const int nb_points) const +{ + std::vector points; + points.reserve(nb_points); + const double step = 1.0 / (nb_points - 1); + for (double t = 0.0; t <= 1.0; t += step) { + Eigen::Vector2d point = valueM(t); + points.emplace_back(point.x(), point.y(), heading(t)); + } + return points; +} + +Eigen::Vector2d Bezier::velocity(const double t) const +{ + Eigen::Matrix ts; + ts << 1, t, t * t, t * t * t, t * t * t * t; + return ts * quintic_bezier_velocity_coefficients * control_points_; +} + +Eigen::Vector2d Bezier::acceleration(const double t) const +{ + Eigen::Matrix ts; + ts << 1, t, t * t, t * t * t; + return ts * quintic_bezier_acceleration_coefficients * control_points_; +} + +double Bezier::curvature(const double t) const +{ + const Eigen::Vector2d vel = velocity(t); + const Eigen::Vector2d accel = acceleration(t); + double curvature = std::numeric_limits::infinity(); + const double denominator = std::pow(vel.x() * vel.x() + vel.y() * vel.y(), 3.0 / 2.0); + if (denominator != 0) curvature = (vel.x() * accel.y() - accel.x() * vel.y()) / denominator; + return curvature; +} + +double Bezier::heading(const double t) const +{ + const Eigen::Vector2d vel = velocity(t); + return std::atan2(vel.y(), vel.x()); +} + +} // namespace bezier_sampler diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp new file mode 100644 index 0000000000000..a9fa318980250 --- /dev/null +++ b/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp @@ -0,0 +1,78 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace bezier_sampler +{ +std::vector sample( + const sampler_common::State & initial, const sampler_common::State & final, + const SamplingParameters & params) +{ + std::vector curves; + curves.reserve(params.nb_t * params.nb_t * params.nb_k); + + double distance_initial_to_final = std::sqrt( + (initial.pose.x() - final.pose.x()) * (initial.pose.x() - final.pose.x()) + + (initial.pose.y() - final.pose.y()) * (initial.pose.y() - final.pose.y())); + // Tangent vectors + const Eigen::Vector2d initial_tangent_unit(std::cos(initial.heading), std::sin(initial.heading)); + const Eigen::Vector2d final_tangent_unit(std::cos(final.heading), std::sin(final.heading)); + // Unit vectors + const Eigen::Vector2d initial_normal_unit = initial_tangent_unit.unitOrthogonal(); + const Eigen::Vector2d final_normal_unit = final_tangent_unit.unitOrthogonal(); + + double step_t = (params.mt_max - params.mt_min) / (params.nb_t - 1); + double step_k = (params.mk_max - params.mk_min) / (params.nb_k - 1); + for (double m_initial = params.mt_min; m_initial <= params.mt_max; m_initial += step_t) { + double initial_tangent_length = m_initial * distance_initial_to_final; + for (double m_final = params.mt_min; m_final <= params.mt_max; m_final += step_t) { + double final_tangent_length = m_final * distance_initial_to_final; + for (double k = params.mk_min; k <= params.mk_max; k += step_k) { + double acceleration_length = k * distance_initial_to_final; + Eigen::Vector2d initial_acceleration = + acceleration_length * initial_tangent_unit + + initial.curvature * initial_tangent_length * initial_tangent_length * initial_normal_unit; + Eigen::Vector2d final_acceleration = + acceleration_length * final_tangent_unit + + final.curvature * final_tangent_length * final_tangent_length * final_normal_unit; + curves.push_back(generate( + {initial.pose.x(), initial.pose.y()}, {final.pose.x(), final.pose.y()}, + initial_tangent_unit * initial_tangent_length, initial_acceleration, + final_tangent_unit * final_tangent_length, final_acceleration)); + } + } + } + return curves; +} +Bezier generate( + const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, + const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration, + const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration) +{ + Eigen::Matrix control_points; + // P0 and P5 correspond to the initial and final configurations + control_points.row(0) = initial_pose; + control_points.row(5) = final_pose; + // P1 and P4 depend on P0, P5, and the initial/final velocities + control_points.row(1) = control_points.row(0) + (1.0 / 5.0) * initial_velocity.transpose(); + control_points.row(4) = control_points.row(5) - (1.0 / 5.0) * final_velocity.transpose(); + // P2 and P3 depend on P1, P4, and the initial/final accelerations + control_points.row(2) = 2 * control_points.row(1) - control_points.row(0) + + (1.0 / 20.0) * initial_acceleration.transpose(); + control_points.row(3) = 2 * control_points.row(4) - control_points.row(5) - + (1.0 / 20.0) * final_acceleration.transpose(); + return Bezier(control_points); +} +} // namespace bezier_sampler diff --git a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt b/planning/sampling_based_planner/frenet_planner/CMakeLists.txt new file mode 100644 index 0000000000000..0d7918daa215b --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +project(frenet_planner) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(frenet_planner SHARED + DIRECTORY + src/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/sampling_based_planner/frenet_planner/README.md b/planning/sampling_based_planner/frenet_planner/README.md new file mode 100644 index 0000000000000..083a880924604 --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/README.md @@ -0,0 +1,7 @@ +# Frenet planner + +Trajectory generation in Frenet frame. + +## Description + +[Original paper](https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame) diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp new file mode 100644 index 0000000000000..f8575d8f65936 --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FRENET_PLANNER__CONVERSIONS_HPP_ +#define FRENET_PLANNER__CONVERSIONS_HPP_ + +#include "frenet_planner/polynomials.hpp" + +#include + +#include + +namespace frenet_planner +{ + +/// @brief calculate the lengths and yaws vectors of the given trajectory +/// @param [inout] trajectory trajectory +inline void calculateLengthsAndYaws(Trajectory & trajectory) +{ + if (trajectory.points.empty()) return; + trajectory.lengths.clear(); + trajectory.yaws.clear(); + trajectory.yaws.reserve(trajectory.points.size()); + trajectory.lengths.reserve(trajectory.points.size()); + + trajectory.lengths.push_back(0.0); + for (auto it = trajectory.points.begin(); it != std::prev(trajectory.points.end()); ++it) { + const auto dx = std::next(it)->x() - it->x(); + const auto dy = std::next(it)->y() - it->y(); + trajectory.lengths.push_back(trajectory.lengths.back() + std::hypot(dx, dy)); + trajectory.yaws.push_back(std::atan2(dy, dx)); + } + const auto last_yaw = trajectory.yaws.empty() ? 0.0 : trajectory.yaws.back(); + trajectory.yaws.push_back(last_yaw); +} +} // namespace frenet_planner + +#endif // FRENET_PLANNER__CONVERSIONS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp new file mode 100644 index 0000000000000..c98e6d97969d4 --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp @@ -0,0 +1,67 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_ +#define FRENET_PLANNER__FRENET_PLANNER_HPP_ + +#include "frenet_planner/structures.hpp" +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +#include +#include + +namespace frenet_planner +{ +/// @brief generate trajectories relative to the reference for the given initial state and sampling +/// parameters +std::vector generateTrajectories( + const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, + const SamplingParameters & sampling_parameters); +/// @brief generate trajectories relative to the reference for the given initial state and sampling +/// parameters +std::vector generateLowVelocityTrajectories( + const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, + const SamplingParameters & sampling_parameters); +/// @brief generate paths relative to the reference for the given initial state and sampling +/// parameters +std::vector generatePaths( + const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, + const SamplingParameters & sampling_parameters); +/// @brief generate a candidate path +/// @details one polynomial for lateral motion (d) is calculated over the longitudinal displacement +/// (s): d(s). +Path generateCandidate( + const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution); +/// @brief generate a candidate trajectory +/// @details the polynomials for lateral motion (d) and longitudinal motion (s) are calculated over +/// time: d(t) and s(t). +Trajectory generateCandidate( + const FrenetState & initial_state, const FrenetState & target_state, const double duration, + const double time_resolution); +/// @brief generate a low velocity candidate trajectory +/// @details the polynomial for lateral motion (d) is calculated over the longitudinal displacement +/// (s) rather than over time: d(s) and s(t). +Trajectory generateLowVelocityCandidate( + const FrenetState & initial_state, const FrenetState & target_state, const double duration, + const double time_resolution); +/// @brief calculate the cartesian frame of the given path +void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path); +/// @brief calculate the cartesian frame of the given trajectory +void calculateCartesian( + const sampler_common::transform::Spline2D & reference, Trajectory & trajectory); + +} // namespace frenet_planner + +#endif // FRENET_PLANNER__FRENET_PLANNER_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp new file mode 100644 index 0000000000000..65157c76d5db3 --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FRENET_PLANNER__POLYNOMIALS_HPP_ +#define FRENET_PLANNER__POLYNOMIALS_HPP_ + +namespace frenet_planner +{ +class Polynomial +{ + /// @brief polynomial coefficients + double a_; + double b_; + double c_; + double d_; + double e_; + double f_; + +public: + /// @brief Create a quintic polynomial between an initial and final state over a parameter length + /// @param x0 initial position + /// @param x0v initial velocity + /// @param x0a initial acceleration + /// @param xT final position + /// @param xTv final velocity + /// @param xTa final acceleration + /// @param T parameter length (arc length or duration) + Polynomial( + const double x0, const double x0v, const double x0a, const double xT, const double xTv, + const double xTa, const double T); + // TODO(Maxime CLEMENT) add quartic case for when final position is not given + + /// @brief Get the position at the given time + [[nodiscard]] double position(const double t) const; + /// @brief Get the velocity at the given time + [[nodiscard]] double velocity(const double t) const; + /// @brief Get the acceleration at the given time + [[nodiscard]] double acceleration(const double t) const; + /// @brief Get the jerk at the given time + [[nodiscard]] double jerk(const double t) const; +}; +} // namespace frenet_planner + +#endif // FRENET_PLANNER__POLYNOMIALS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp new file mode 100644 index 0000000000000..848d8f11938cf --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp @@ -0,0 +1,156 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FRENET_PLANNER__STRUCTURES_HPP_ +#define FRENET_PLANNER__STRUCTURES_HPP_ + +#include "frenet_planner/polynomials.hpp" + +#include + +#include +#include + +namespace frenet_planner +{ + +struct FrenetState +{ + sampler_common::FrenetPoint position = {0, 0}; + double lateral_velocity{}; + double longitudinal_velocity{}; + double lateral_acceleration{}; + double longitudinal_acceleration{}; +}; + +struct Path : sampler_common::Path +{ + std::vector frenet_points{}; + std::optional lateral_polynomial{}; + + Path() = default; + explicit Path(const sampler_common::Path & path) : sampler_common::Path(path) {} + + void clear() override + { + sampler_common::Path::clear(); + frenet_points.clear(); + lateral_polynomial.reset(); + } + + void reserve(const size_t size) override + { + sampler_common::Path::reserve(size); + frenet_points.reserve(size); + } + + [[nodiscard]] Path extend(const Path & path) const + { + Path extended_traj(sampler_common::Path::extend(path)); + extended_traj.frenet_points.insert( + extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); + extended_traj.frenet_points.insert( + extended_traj.frenet_points.end(), path.frenet_points.begin(), path.frenet_points.end()); + // TODO(Maxime CLEMENT): direct copy from the 2nd trajectory. might need to be improved + extended_traj.lateral_polynomial = path.lateral_polynomial; + return extended_traj; + } + + [[nodiscard]] Path * subset(const size_t from_idx, const size_t to_idx) const override + { + auto * subpath = new Path(*sampler_common::Path::subset(from_idx, to_idx)); + assert(to_idx >= from_idx); + subpath->reserve(to_idx - from_idx); + + const auto copy_subset = [&](const auto & from, auto & to) { + to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); + }; + copy_subset(frenet_points, subpath->frenet_points); + return subpath; + }; +}; + +struct SamplingParameter +{ + double target_duration{}; + FrenetState target_state; +}; + +struct Trajectory : sampler_common::Trajectory +{ + std::vector frenet_points{}; + std::optional lateral_polynomial{}; + std::optional longitudinal_polynomial{}; + SamplingParameter sampling_parameter; + + Trajectory() = default; + explicit Trajectory(const sampler_common::Trajectory & traj) : sampler_common::Trajectory(traj) {} + + void clear() override + { + sampler_common::Trajectory::clear(); + frenet_points.clear(); + lateral_polynomial.reset(); + longitudinal_polynomial.reset(); + } + + void reserve(const size_t size) override + { + sampler_common::Trajectory::reserve(size); + frenet_points.reserve(size); + } + + [[nodiscard]] Trajectory extend(const Trajectory & traj) const + { + Trajectory extended_traj(sampler_common::Trajectory::extend(traj)); + extended_traj.frenet_points.insert( + extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); + extended_traj.frenet_points.insert( + extended_traj.frenet_points.end(), traj.frenet_points.begin(), traj.frenet_points.end()); + // TODO(Maxime CLEMENT): direct copy from the 2nd trajectory. might need to be improved + extended_traj.lateral_polynomial = traj.lateral_polynomial; + extended_traj.longitudinal_polynomial = traj.longitudinal_polynomial; + return extended_traj; + } + + [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override + { + auto * subtraj = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); + assert(to_idx >= from_idx); + subtraj->reserve(to_idx - from_idx); + + const auto copy_subset = [&](const auto & from, auto & to) { + to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); + }; + copy_subset(frenet_points, subtraj->frenet_points); + return subtraj; + }; +}; + +inline std::ostream & operator<<(std::ostream & os, const SamplingParameter & sp) +{ + const auto & s = sp.target_state; + return os << "[" + << "T=" << sp.target_duration << ", s=" << s.position.s << ", d=" << s.position.d + << ", s'=" << s.longitudinal_velocity << ", d'=" << s.lateral_velocity + << ", s\"=" << s.longitudinal_acceleration << ", d\"=" << s.lateral_acceleration << "]"; +} +struct SamplingParameters +{ + std::vector parameters; + double resolution; +}; +} // namespace frenet_planner + +#endif // FRENET_PLANNER__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch b/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch new file mode 100644 index 0000000000000..9570d488eaecc --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/planning/sampling_based_planner/frenet_planner/package.xml b/planning/sampling_based_planner/frenet_planner/package.xml new file mode 100644 index 0000000000000..c378c6ae25e9b --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/package.xml @@ -0,0 +1,24 @@ + + + frenet_planner + 0.0.1 + The frenet_planner package for trajectory generation in Frenet frame + Maxime CLEMENT + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_auto_common + sampler_common + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp new file mode 100644 index 0000000000000..ebd628793fa24 --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -0,0 +1,227 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "frenet_planner/frenet_planner.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace frenet_planner +{ +std::vector generateTrajectories( + const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, + const SamplingParameters & sampling_parameters) +{ + std::vector trajectories; + trajectories.reserve(sampling_parameters.parameters.size()); + for (const auto & parameter : sampling_parameters.parameters) { + auto trajectory = generateCandidate( + initial_state, parameter.target_state, parameter.target_duration, + sampling_parameters.resolution); + trajectory.sampling_parameter = parameter; + calculateCartesian(reference_spline, trajectory); + std::stringstream ss; + ss << parameter; + trajectory.tag = ss.str(); + trajectories.push_back(trajectory); + } + return trajectories; +} + +std::vector generateLowVelocityTrajectories( + const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, + const SamplingParameters & sampling_parameters) +{ + std::vector trajectories; + trajectories.reserve(sampling_parameters.parameters.size()); + for (const auto & parameter : sampling_parameters.parameters) { + auto trajectory = generateLowVelocityCandidate( + initial_state, parameter.target_state, parameter.target_duration, + sampling_parameters.resolution); + calculateCartesian(reference_spline, trajectory); + std::stringstream ss; + ss << parameter; + trajectory.tag = ss.str(); + trajectories.push_back(trajectory); + } + return trajectories; +} + +std::vector generatePaths( + const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, + const SamplingParameters & sampling_parameters) +{ + std::vector candidates; + candidates.reserve(sampling_parameters.parameters.size()); + for (const auto parameter : sampling_parameters.parameters) { + auto candidate = + generateCandidate(initial_state, parameter.target_state, sampling_parameters.resolution); + calculateCartesian(reference_spline, candidate); + candidates.push_back(candidate); + } + return candidates; +} + +Trajectory generateCandidate( + const FrenetState & initial_state, const FrenetState & target_state, const double duration, + const double time_resolution) +{ + Trajectory trajectory; + trajectory.longitudinal_polynomial = Polynomial( + initial_state.position.s, initial_state.longitudinal_velocity, + initial_state.longitudinal_acceleration, target_state.position.s, + target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration); + trajectory.lateral_polynomial = Polynomial( + initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration, + target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration, + duration); + for (double t = 0.0; t <= duration; t += time_resolution) { + trajectory.times.push_back(t); + trajectory.frenet_points.emplace_back( + trajectory.longitudinal_polynomial->position(t), trajectory.lateral_polynomial->position(t)); + } + return trajectory; +} + +Trajectory generateLowVelocityCandidate( + const FrenetState & initial_state, const FrenetState & target_state, const double duration, + const double time_resolution) +{ + Trajectory trajectory; + trajectory.longitudinal_polynomial = Polynomial( + initial_state.position.s, initial_state.longitudinal_velocity, + initial_state.longitudinal_acceleration, target_state.position.s, + target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration); + const auto delta_s = target_state.position.s - initial_state.position.s; + trajectory.lateral_polynomial = Polynomial( + initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration, + target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration, + delta_s); + for (double t = 0.0; t <= duration; t += time_resolution) { + trajectory.times.push_back(t); + const auto s = trajectory.longitudinal_polynomial->position(t); + const auto ds = s - initial_state.position.s; + const auto d = trajectory.lateral_polynomial->position(ds); + trajectory.frenet_points.emplace_back(s, d); + } + return trajectory; +} + +Path generateCandidate( + const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution) +{ + const auto delta_s = target_state.position.s - initial_state.position.s; + Path path; + path.lateral_polynomial = Polynomial( + initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration, + target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration, + delta_s); + for (double s = s_resolution; s <= delta_s; s += s_resolution) { + path.frenet_points.emplace_back( + initial_state.position.s + s, path.lateral_polynomial->position(s)); + } + return path; +} + +void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path) +{ + if (!path.frenet_points.empty()) { + path.points.reserve(path.frenet_points.size()); + path.yaws.reserve(path.frenet_points.size()); + path.lengths.reserve(path.frenet_points.size()); + path.curvatures.reserve(path.frenet_points.size()); + // Calculate cartesian positions + for (const auto & fp : path.frenet_points) { + path.points.push_back(reference.cartesian(fp)); + } + // TODO(Maxime CLEMENT): more precise calculations are proposed in Appendix I of the paper: + // Optimal path Generation for Dynamic Street Scenarios in a Frenet Frame (Werling2010) + // Calculate cartesian yaw and interval values + path.lengths.push_back(0.0); + for (auto it = path.points.begin(); it != std::prev(path.points.end()); ++it) { + const auto dx = std::next(it)->x() - it->x(); + const auto dy = std::next(it)->y() - it->y(); + path.yaws.push_back(std::atan2(dy, dx)); + path.lengths.push_back(path.lengths.back() + std::hypot(dx, dy)); + } + path.yaws.push_back(path.yaws.back()); + // Calculate curvatures + for (size_t i = 1; i < path.yaws.size(); ++i) { + const auto dyaw = + autoware::common::helper_functions::wrap_angle(path.yaws[i] - path.yaws[i - 1]); + path.curvatures.push_back(dyaw / (path.lengths[i - 1], path.lengths[i])); + } + path.curvatures.push_back(path.curvatures.back()); + } +} +void calculateCartesian( + const sampler_common::transform::Spline2D & reference, Trajectory & trajectory) +{ + if (!trajectory.frenet_points.empty()) { + trajectory.points.reserve(trajectory.frenet_points.size()); + // Calculate cartesian positions + for (const auto & fp : trajectory.frenet_points) + trajectory.points.push_back(reference.cartesian(fp)); + calculateLengthsAndYaws(trajectory); + std::vector dyaws; + dyaws.reserve(trajectory.yaws.size()); + for (size_t i = 0; i + 1 < trajectory.yaws.size(); ++i) + dyaws.push_back(autoware::common::helper_functions::wrap_angle( + trajectory.yaws[i + 1] - trajectory.yaws[i])); + dyaws.push_back(0.0); + // Calculate curvatures + for (size_t i = 1; i < trajectory.yaws.size(); ++i) { + const auto curvature = trajectory.lengths[i] == trajectory.lengths[i - 1] + ? 0.0 + : dyaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]); + trajectory.curvatures.push_back(curvature); + } + const auto last_curvature = trajectory.curvatures.empty() ? 0.0 : trajectory.curvatures.back(); + trajectory.curvatures.push_back(last_curvature); + // Calculate velocities, accelerations, jerk + for (size_t i = 0; i < trajectory.times.size(); ++i) { + const auto time = trajectory.times[i]; + const auto s_vel = trajectory.longitudinal_polynomial->velocity(time); + const auto s_acc = trajectory.longitudinal_polynomial->acceleration(time); + const auto d_vel = trajectory.lateral_polynomial->velocity(time); + const auto d_acc = trajectory.lateral_polynomial->acceleration(time); + Eigen::Rotation2D rotation(dyaws[i]); + Eigen::Vector2d vel_vector{s_vel, d_vel}; + Eigen::Vector2d acc_vector{s_acc, d_acc}; + const auto vel = rotation * vel_vector; + const auto acc = rotation * acc_vector; + trajectory.longitudinal_velocities.push_back(vel.x()); + trajectory.lateral_velocities.push_back(vel.y()); + trajectory.longitudinal_accelerations.push_back(acc.x()); + trajectory.lateral_accelerations.push_back(acc.y()); + trajectory.jerks.push_back( + trajectory.longitudinal_polynomial->jerk(time) + trajectory.lateral_polynomial->jerk(time)); + } + if (trajectory.longitudinal_accelerations.empty()) { + trajectory.longitudinal_accelerations.push_back(0.0); + trajectory.lateral_accelerations.push_back(0.0); + } + } +} + +} // namespace frenet_planner diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp new file mode 100644 index 0000000000000..13f71dc88f9d6 --- /dev/null +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace frenet_planner +{ +// @brief Create a polynomial connecting the given initial and target configuration over the given +// duration +Polynomial::Polynomial( + const double x0, const double x0v, const double x0a, const double xT, const double xTv, + const double xTa, const double T) +{ + d_ = x0a / 2; + e_ = x0v; + f_ = x0; + + const double T2 = T * T; + const double T3 = T2 * T; + const double T4 = T3 * T; + const double T5 = T4 * T; + + Eigen::Matrix3d A; + A << T5, T4, T3, 5 * T4, 4 * T3, 3 * T2, 20 * T3, 12 * T2, 6 * T; + Eigen::Vector3d B; + B << xT - x0 - x0v * T - 0.5 * x0a * T2, xTv - x0v - x0a * T, xTa - x0a; + + const Eigen::Vector3d X = A.colPivHouseholderQr().solve(B); + a_ = X(0); + b_ = X(1); + c_ = X(2); +} + +double Polynomial::position(const double t) const +{ + const double t2 = t * t; + const double t3 = t2 * t; + const double t4 = t3 * t; + const double t5 = t4 * t; + return a_ * t5 + b_ * t4 + c_ * t3 + d_ * t2 + e_ * t + f_; +} + +double Polynomial::velocity(const double t) const +{ + const double t2 = t * t; + const double t3 = t2 * t; + const double t4 = t3 * t; + return 5 * a_ * t4 + 4 * b_ * t3 + 3 * c_ * t2 + 2 * d_ * t + e_; +} + +double Polynomial::acceleration(const double t) const +{ + const double t2 = t * t; + const double t3 = t2 * t; + return 20 * a_ * t3 + 12 * b_ * t2 + 6 * c_ * t + 2 * d_; +} + +double Polynomial::jerk(const double t) const +{ + const double t2 = t * t; + return 60 * a_ * t2 + 24 * b_ * t + 6 * c_; +} +} // namespace frenet_planner diff --git a/planning/sampling_based_planner/path_sampler/CMakeLists.txt b/planning/sampling_based_planner/path_sampler/CMakeLists.txt new file mode 100644 index 0000000000000..5a5b99f0aff3a --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(path_sampler) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(path_sampler SHARED + DIRECTORY src +) + +# register node +rclcpp_components_register_node(path_sampler + PLUGIN "path_sampler::PathSampler" + EXECUTABLE path_sampler_exe +) + +ament_auto_package( +# INSTALL_TO_SHARE +) diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md new file mode 100644 index 0000000000000..533c79252dfe4 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -0,0 +1,98 @@ +# Path Sampler + +## Purpose + +This package implements a node that uses sampling based planning to generate a drivable trajectry. + +## Feature + +This package is able to: + +- make the trajectory smooth; +- keep the trajectory inside the drivable area; +- avoid static obstacles; +- stop if no valid trajectory can be generated. + +Note that the velocity is just taken over from the input path. + +## Inputs / Outputs + +### input + +| Name | Type | Description | +| ------------------ | -------------------------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | +| `~/input/objects` | autoware_auto_perception_msgs/msg/PredictedObjects | objects to avoid | + +### output + +| Name | Type | Description | +| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | + +## Algorithm + +Sampling based planning is decomposed into 3 successives steps: + +1. Sampling: candidate trajectories are generated. +2. Pruning: invalid candidates are discarded. +3. Selection: the best remaining valid candidate is selected. + +### Sampling + +Candidate trajectories are generated based on the current ego state and some target state. +2 sampling algorithms are currently implemented: sampling with bézier curves or with polynomials in the frenet frame. + +### Pruning + +The validity of each candidate trajectory is checked using a set of hard constraints. + +- collision: ensure no collision with static obstacles; +- curvature: ensure smooth curvature; +- drivable area: ensure the trajectory stays within the drivable area. + +### Selection + +Among the valid candidate trajectories, the _best_ one is determined using a set of soft constraints (i.e., objective functions). + +- curvature: prefer smoother trajectories; +- length: prefer longer trajectories; +- lateral deviation: prefer trajectories close to the reference path. + +Each soft constraint is associated with a weight to allow tuning of the preferences. + +## Limitations + +The quality of the candidates generated with polynomials in frenet frame greatly depend on the reference path. +If the reference path is not smooth, the resulting candidates will probably be undrivable. + +Failure to find a valid trajectory current results in a suddenly stopping trajectory. + +## Comparison with the `obstacle_avoidance_planner` + +The `obstacle_avoidance_planner` uses an optimization based approach, +finding the optimal solution of a mathematical problem if it exists. +When no solution can be found, it is often hard to identify the issue due to the intermediate mathematical representation of the problem. + +In comparison, the sampling based approach cannot guarantee an optimal solution but is much more straightforward, +making it easier to debug and tune. + +## How to Tune Parameters + +The sampling based planner mostly offers a trade-off between the consistent quality of the trajectory and the computation time. +To guarantee that a good trajectory is found requires generating many candidates which linearly increases the computation time. + +TODO + +### Drivability in narrow roads + +### Computation time + +### Robustness + +### Other options + +## How To Debug + +TODO diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp new file mode 100644 index 0000000000000..30d9a77f31fad --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp @@ -0,0 +1,133 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#define PATH_SAMPLER__COMMON_STRUCTS_HPP_ + +#include "path_sampler/type_alias.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sampler_common/structures.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include +#include +#include + +namespace path_sampler +{ +struct PlannerData +{ + // input + Header header; + std::vector traj_points; // converted from the input path + std::vector left_bound; + std::vector right_bound; + + // ego + geometry_msgs::msg::Pose ego_pose; + double ego_vel{}; +}; + +struct TimeKeeper +{ + void init() { accumulated_msg = "\n"; } + + template + TimeKeeper & operator<<(const T & msg) + { + latest_stream << msg; + return *this; + } + + void endLine() + { + const auto msg = latest_stream.str(); + accumulated_msg += msg + "\n"; + latest_stream.str(""); + + if (enable_calculation_time_info) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("path_sampler.time"), msg); + } + } + + void tic(const std::string & func_name) { stop_watch_.tic(func_name); } + + void toc(const std::string & func_name, const std::string & white_spaces) + { + const double elapsed_time = stop_watch_.toc(func_name); + *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + endLine(); + } + + std::string getLog() const { return accumulated_msg; } + + bool enable_calculation_time_info; + std::string accumulated_msg = "\n"; + std::stringstream latest_stream; + + tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; +}; + +struct DebugData +{ + std::vector sampled_candidates{}; + size_t previous_sampled_candidates_nb = 0UL; + std::vector obstacles{}; + std::vector footprints{}; +}; + +struct TrajectoryParam +{ + TrajectoryParam() = default; + explicit TrajectoryParam(rclcpp::Node * node) + { + output_delta_arc_length = node->declare_parameter("common.output_delta_arc_length"); + } + + void onParam(const std::vector & parameters) + { + using tier4_autoware_utils::updateParam; + + // common + updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); + } + + double output_delta_arc_length; +}; + +struct EgoNearestParam +{ + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node * node) + { + dist_threshold = node->declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node->declare_parameter("ego_nearest_yaw_threshold"); + } + + void onParam(const std::vector & parameters) + { + using tier4_autoware_utils::updateParam; + updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); + } + + double dist_threshold{0.0}; + double yaw_threshold{0.0}; +}; +} // namespace path_sampler + +#endif // PATH_SAMPLER__COMMON_STRUCTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp new file mode 100644 index 0000000000000..3555d3b799983 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -0,0 +1,105 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__NODE_HPP_ +#define PATH_SAMPLER__NODE_HPP_ + +#include "motion_utils/motion_utils.hpp" +#include "path_sampler/common_structs.hpp" +#include "path_sampler/parameters.hpp" +#include "path_sampler/type_alias.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sampler_common/transform/spline_transform.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace path_sampler +{ +class PathSampler : public rclcpp::Node +{ +public: + explicit PathSampler(const rclcpp::NodeOptions & node_options); + +protected: // for the static_centerline_optimizer package + // argument variables + vehicle_info_util::VehicleInfo vehicle_info_{}; + mutable DebugData debug_data_{}; + mutable std::shared_ptr time_keeper_ptr_{nullptr}; + + // parameters + TrajectoryParam traj_param_{}; + EgoNearestParam ego_nearest_param_{}; + Parameters params_; + size_t debug_id_ = 0; + + // variables for subscribers + Odometry::SharedPtr ego_state_ptr_; + PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared(); + + // variables for previous information + std::optional prev_path_; + + // interface publisher + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + + // interface subscriber + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr objects_sub_; + + // debug publisher + rclcpp::Publisher::SharedPtr debug_markers_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + + // parameter callback + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + // subscriber callback function + void objectsCallback(const PredictedObjects::SharedPtr msg); + void onPath(const Path::SharedPtr); + + // main functions + bool isDataReady(const Path & path, rclcpp::Clock clock) const; + PlannerData createPlannerData(const Path & path) const; + std::vector generateTrajectory(const PlannerData & planner_data); + std::vector extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_points) const; + void resetPreviousData(); + sampler_common::State getPlanningState( + sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const; + + // sub-functions of generateTrajectory + std::vector generatePath(const PlannerData & planner_data); + void applyInputVelocity( + std::vector & output_traj_points, + const std::vector & input_traj_points, + const geometry_msgs::msg::Pose & ego_pose) const; + void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; + void publishDebugMarker(const std::vector & traj_points) const; +}; +} // namespace path_sampler + +#endif // PATH_SAMPLER__NODE_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp new file mode 100644 index 0000000000000..1c5c51e33b431 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp @@ -0,0 +1,51 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__PARAMETERS_HPP_ +#define PATH_SAMPLER__PARAMETERS_HPP_ + +#include "bezier_sampler/bezier_sampling.hpp" +#include "sampler_common/structures.hpp" + +#include + +struct Parameters +{ + sampler_common::Constraints constraints; + struct + { + bool enable_frenet{}; + bool enable_bezier{}; + double resolution{}; + int previous_path_reuse_points_nb{}; + std::vector target_lengths{}; + std::vector target_lateral_positions{}; + int nb_target_lateral_positions{}; + struct + { + std::vector target_lateral_velocities{}; + std::vector target_lateral_accelerations{}; + } frenet; + bezier_sampler::SamplingParameters bezier{}; + } sampling; + + struct + { + bool force_zero_deviation{}; + bool force_zero_heading{}; + bool smooth_reference{}; + } preprocessing{}; +}; + +#endif // PATH_SAMPLER__PARAMETERS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp new file mode 100644 index 0000000000000..41493b74659fc --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__PATH_GENERATION_HPP_ +#define PATH_SAMPLER__PATH_GENERATION_HPP_ + +#include "bezier_sampler/bezier_sampling.hpp" +#include "frenet_planner/structures.hpp" +#include "path_sampler/parameters.hpp" +#include "sampler_common/constraints/hard_constraint.hpp" +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +#include + +#include + +namespace path_sampler +{ +/** + * @brief generate candidate paths for the given problem inputs + * @param [in] initial_state initial ego state + * @param [in] path_spline spline of the reference path + * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) + * @param [in] params parameters + * @return candidate paths + */ +std::vector generateCandidatePaths( + const sampler_common::State & initial_state, + const sampler_common::transform::Spline2D & path_spline, const double base_length, + const Parameters & params); + +std::vector generateBezierPaths( + const sampler_common::State & initial_state, const double base_length, + const sampler_common::transform::Spline2D & path_spline, const Parameters & params); +std::vector generateFrenetPaths( + const sampler_common::State & initial_state, const double base_length, + const sampler_common::transform::Spline2D & path_spline, const Parameters & params); +} // namespace path_sampler + +#endif // PATH_SAMPLER__PATH_GENERATION_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp new file mode 100644 index 0000000000000..29cfddfbf8632 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#define PATH_SAMPLER__PREPARE_INPUTS_HPP_ + +#include "frenet_planner/structures.hpp" +#include "path_sampler/parameters.hpp" +#include "path_sampler/type_alias.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace path_sampler +{ +/// @brief prepare constraints +void prepareConstraints( + sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + const std::vector & left_bound, + const std::vector & right_bound); +/// @brief prepare sampling parameters to generate Frenet paths +frenet_planner::SamplingParameters prepareSamplingParameters( + const sampler_common::State & initial_state, const double base_length, + const sampler_common::transform::Spline2D & path_spline, const Parameters & params); +/// @brief prepare the 2D spline representation of the given Path message +sampler_common::transform::Spline2D preparePathSpline( + const std::vector & path_msg, const bool smooth_path); +} // namespace path_sampler + +#endif // PATH_SAMPLER__PREPARE_INPUTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp new file mode 100644 index 0000000000000..8a792eba2f237 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp @@ -0,0 +1,51 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ +#define PATH_SAMPLER__TYPE_ALIAS_HPP_ + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/string_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace path_sampler +{ +// std_msgs +using std_msgs::msg::Header; +// planning +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +// perception +using autoware_auto_perception_msgs::msg::PredictedObjects; +// navigation +using nav_msgs::msg::Odometry; +// visualization +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +// debug +using tier4_debug_msgs::msg::StringStamped; +} // namespace path_sampler + +#endif // PATH_SAMPLER__TYPE_ALIAS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp new file mode 100644 index 0000000000000..3e5afdbdad696 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "path_sampler/common_structs.hpp" +#include "path_sampler/type_alias.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include +#include +#include + +namespace path_sampler +{ +namespace geometry_utils +{ +template +bool isSamePoint(const T1 & t1, const T2 & t2) +{ + const auto p1 = tier4_autoware_utils::getPoint(t1); + const auto p2 = tier4_autoware_utils::getPoint(t2); + + constexpr double epsilon = 1e-6; + if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { + return false; + } + return true; +} +} // namespace geometry_utils +} // namespace path_sampler +#endif // PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp new file mode 100644 index 0000000000000..c0a3c1d917c25 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp @@ -0,0 +1,200 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "path_sampler/common_structs.hpp" +#include "path_sampler/type_alias.hpp" +#include "sampler_common/structures.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include +#include +#include +#include +#include + +namespace path_sampler +{ +namespace trajectory_utils +{ +template +std::optional getPointIndexAfter( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double offset) +{ + if (points.empty()) { + return std::nullopt; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + + // search forward + if (sum_length < offset) { + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (offset < sum_length) { + return i - 1; + } + } + + return std::nullopt; + } + + // search backward + for (size_t i = target_seg_idx; 0 < i; + --i) { // NOTE: use size_t since i is always positive value + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < offset) { + return i - 1; + } + } + + return 0; +} + +template +TrajectoryPoint convertToTrajectoryPoint(const T & point) +{ + TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + return traj_point; +} + +template +std::vector convertToTrajectoryPoints(const std::vector & points) +{ + std::vector traj_points; + for (const auto & point : points) { + const auto traj_point = convertToTrajectoryPoint(point); + traj_points.push_back(traj_point); + } + return traj_points; +} + +inline std::vector convertToTrajectoryPoints(const sampler_common::Path & path) +{ + std::vector traj_points; + for (auto i = 0UL; i < path.points.size(); ++i) { + TrajectoryPoint p; + p.pose.position.x = path.points[i].x(); + p.pose.position.y = path.points[i].y(); + auto quat = tf2::Quaternion(); + quat.setRPY(0.0, 0.0, path.yaws[i]); + p.pose.orientation.w = quat.w(); + p.pose.orientation.x = quat.x(); + p.pose.orientation.y = quat.y(); + p.pose.orientation.z = quat.z(); + p.longitudinal_velocity_mps = 0.0; + p.lateral_velocity_mps = 0.0; + p.heading_rate_rps = 0.0; + traj_points.push_back(p); + } + return traj_points; +} + +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold); + +template +size_t findEgoIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, + const EgoNearestParam & ego_nearest_param) +{ + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); +} + +template +size_t findEgoSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, + const EgoNearestParam & ego_nearest_param) +{ + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points); + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval); + +std::vector resampleTrajectoryPointsWithoutStopPoint( + const std::vector traj_points, const double interval); + +template +std::optional updateFrontPointForFix( + std::vector & points, std::vector & points_for_fix, const double delta_arc_length, + const EgoNearestParam & ego_nearest_param) +{ + // calculate front point to insert in points as a fixed point + const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( + points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + const size_t front_point_idx_for_fix = front_seg_idx_for_fix; + const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); + + // check if the points_for_fix is longer in front than points + const double lon_offset_to_prev_front = + motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + if (0 < lon_offset_to_prev_front) { + RCLCPP_WARN( + rclcpp::get_logger("path_sampler.trajectory_utils"), + "Fixed point will not be inserted due to the error during calculation."); + return std::nullopt; + } + + const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + + // check if deviation is not too large + constexpr double max_lat_error = 3.0; + if (max_lat_error < dist) { + RCLCPP_WARN( + rclcpp::get_logger("path_sampler.trajectory_utils"), + "New Fixed point is too far from points %f [m]", dist); + } + + // update pose + if (dist < delta_arc_length) { + // only pose is updated + points.front() = front_fix_point; + } else { + // add new front point + T new_front_point; + new_front_point = front_fix_point; + points.insert(points.begin(), new_front_point); + } + + return front_point_idx_for_fix; +} + +void insertStopPoint( + std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, + const size_t stop_seg_idx); +} // namespace trajectory_utils +} // namespace path_sampler +#endif // PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/path_sampler/package.xml new file mode 100644 index 0000000000000..22303a021ff63 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/package.xml @@ -0,0 +1,43 @@ + + + + path_sampler + 0.1.0 + Package for the sampling-based path planner + Maxime CLEMENT + Apache License 2.0 + + Maxime CLEMENT + + ament_cmake_auto + + autoware_cmake + + autoware_auto_planning_msgs + bezier_sampler + frenet_planner + geometry_msgs + interpolation + motion_utils + nav_msgs + rclcpp + rclcpp_components + std_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + fake_test_node + + + ament_cmake + + diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp new file mode 100644 index 0000000000000..298bdf76a7c58 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -0,0 +1,647 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "path_sampler/node.hpp" + +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "path_sampler/path_generation.hpp" +#include "path_sampler/prepare_inputs.hpp" +#include "path_sampler/utils/geometry_utils.hpp" +#include "path_sampler/utils/trajectory_utils.hpp" +#include "rclcpp/time.hpp" +#include "sampler_common/constraints/hard_constraint.hpp" +#include "sampler_common/constraints/soft_constraint.hpp" + +#include +#include + +namespace path_sampler +{ +namespace +{ +template +std::vector concatVectors(const std::vector & prev_vector, const std::vector & next_vector) +{ + std::vector concatenated_vector; + concatenated_vector.insert(concatenated_vector.end(), prev_vector.begin(), prev_vector.end()); + concatenated_vector.insert(concatenated_vector.end(), next_vector.begin(), next_vector.end()); + return concatenated_vector; +} + +StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) +{ + StringStamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + +bool hasZeroVelocity(const TrajectoryPoint & traj_point) +{ + constexpr double zero_vel = 0.0001; + return std::abs(traj_point.longitudinal_velocity_mps) < zero_vel; +} +} // namespace + +PathSampler::PathSampler(const rclcpp::NodeOptions & node_options) +: Node("path_sampler", node_options), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + time_keeper_ptr_(std::make_shared()) +{ + // interface publisher + traj_pub_ = create_publisher("~/output/path", 1); + virtual_wall_pub_ = create_publisher("~/virtual_wall", 1); + + // interface subscriber + path_sub_ = create_subscription( + "~/input/path", 1, std::bind(&PathSampler::onPath, this, std::placeholders::_1)); + odom_sub_ = create_subscription( + "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + objects_sub_ = create_subscription( + "~/input/objects", 1, std::bind(&PathSampler::objectsCallback, this, std::placeholders::_1)); + + // debug publisher + debug_markers_pub_ = create_publisher("~/debug/marker", 1); + debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + + { // parameters + params_.constraints.hard.max_curvature = + declare_parameter("constraints.hard.max_curvature"); + params_.constraints.hard.min_curvature = + declare_parameter("constraints.hard.min_curvature"); + params_.constraints.soft.lateral_deviation_weight = + declare_parameter("constraints.soft.lateral_deviation_weight"); + params_.constraints.soft.length_weight = + declare_parameter("constraints.soft.length_weight"); + params_.constraints.soft.curvature_weight = + declare_parameter("constraints.soft.curvature_weight"); + params_.sampling.enable_frenet = declare_parameter("sampling.enable_frenet"); + params_.sampling.enable_bezier = declare_parameter("sampling.enable_bezier"); + params_.sampling.resolution = declare_parameter("sampling.resolution"); + params_.sampling.previous_path_reuse_points_nb = + declare_parameter("sampling.previous_path_reuse_points_nb"); + params_.sampling.target_lengths = + declare_parameter>("sampling.target_lengths"); + params_.sampling.target_lateral_positions = + declare_parameter>("sampling.target_lateral_positions"); + params_.sampling.nb_target_lateral_positions = + declare_parameter("sampling.nb_target_lateral_positions"); + params_.sampling.frenet.target_lateral_velocities = + declare_parameter>("sampling.frenet.target_lateral_velocities"); + params_.sampling.frenet.target_lateral_accelerations = + declare_parameter>("sampling.frenet.target_lateral_accelerations"); + params_.sampling.bezier.nb_k = declare_parameter("sampling.bezier.nb_k"); + params_.sampling.bezier.mk_min = declare_parameter("sampling.bezier.mk_min"); + params_.sampling.bezier.mk_max = declare_parameter("sampling.bezier.mk_max"); + params_.sampling.bezier.nb_t = declare_parameter("sampling.bezier.nb_t"); + params_.sampling.bezier.mt_min = declare_parameter("sampling.bezier.mt_min"); + params_.sampling.bezier.mt_max = declare_parameter("sampling.bezier.mt_max"); + params_.preprocessing.force_zero_deviation = + declare_parameter("preprocessing.force_zero_initial_deviation"); + params_.preprocessing.force_zero_heading = + declare_parameter("preprocessing.force_zero_initial_heading"); + params_.preprocessing.smooth_reference = + declare_parameter("preprocessing.smooth_reference_trajectory"); + params_.constraints.ego_footprint = vehicle_info_.createFootprint(); + params_.constraints.ego_width = vehicle_info_.vehicle_width_m; + params_.constraints.ego_length = vehicle_info_.vehicle_length_m; + + // parameter for debug info + time_keeper_ptr_->enable_calculation_time_info = + declare_parameter("debug.enable_calculation_time_info"); + debug_id_ = static_cast(declare_parameter("debug.id")); + + // parameters for ego nearest search + ego_nearest_param_ = EgoNearestParam(this); + + // parameters for trajectory + traj_param_ = TrajectoryParam(this); + } + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PathSampler::onParam, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult PathSampler::onParam( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "constraints.hard.max_curvature", params_.constraints.hard.max_curvature); + updateParam(parameters, "constraints.hard.min_curvature", params_.constraints.hard.min_curvature); + updateParam( + parameters, "constraints.soft.lateral_deviation_weight", + params_.constraints.soft.lateral_deviation_weight); + updateParam(parameters, "constraints.soft.length_weight", params_.constraints.soft.length_weight); + updateParam( + parameters, "constraints.soft.curvature_weight", params_.constraints.soft.curvature_weight); + updateParam(parameters, "sampling.enable_frenet", params_.sampling.enable_frenet); + updateParam(parameters, "sampling.enable_bezier", params_.sampling.enable_bezier); + updateParam(parameters, "sampling.resolution", params_.sampling.resolution); + updateParam( + parameters, "sampling.previous_path_reuse_points_nb", + params_.sampling.previous_path_reuse_points_nb); + updateParam(parameters, "sampling.target_lengths", params_.sampling.target_lengths); + updateParam( + parameters, "sampling.target_lateral_positions", params_.sampling.target_lateral_positions); + updateParam( + parameters, "sampling.nb_target_lateral_positions", + params_.sampling.nb_target_lateral_positions); + updateParam( + parameters, "sampling.frenet.target_lateral_velocities", + params_.sampling.frenet.target_lateral_velocities); + updateParam( + parameters, "sampling.frenet.target_lateral_accelerations", + params_.sampling.frenet.target_lateral_accelerations); + updateParam(parameters, "sampling.bezier.nb_k", params_.sampling.bezier.nb_k); + updateParam(parameters, "sampling.bezier.mk_min", params_.sampling.bezier.mk_min); + updateParam(parameters, "sampling.bezier.mk_max", params_.sampling.bezier.mk_max); + updateParam(parameters, "sampling.bezier.nb_t", params_.sampling.bezier.nb_t); + updateParam(parameters, "sampling.bezier.mt_min", params_.sampling.bezier.mt_min); + updateParam(parameters, "sampling.bezier.mt_max", params_.sampling.bezier.mt_max); + updateParam( + parameters, "preprocessing.force_zero_initial_deviation", + params_.preprocessing.force_zero_deviation); + updateParam( + parameters, "preprocessing.force_zero_initial_heading", + params_.preprocessing.force_zero_heading); + updateParam( + parameters, "preprocessing.smooth_reference_trajectory", + params_.preprocessing.smooth_reference); + updateParam( + parameters, "debug.enable_calculation_time_info", + time_keeper_ptr_->enable_calculation_time_info); + updateParam(parameters, "debug.id", debug_id_); + // parameters for ego nearest search + ego_nearest_param_.onParam(parameters); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +void PathSampler::resetPreviousData() +{ + prev_path_.reset(); +} + +void PathSampler::objectsCallback(const PredictedObjects::SharedPtr msg) +{ + in_objects_ptr_ = msg; +} + +sampler_common::State PathSampler::getPlanningState( + sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const +{ + state.frenet = path_spline.frenet(state.pose); + if (params_.preprocessing.force_zero_deviation) { + state.pose = path_spline.cartesian(state.frenet.s); + } + if (params_.preprocessing.force_zero_heading) { + state.heading = path_spline.yaw(state.frenet.s); + } + state.curvature = path_spline.curvature(state.frenet.s); + return state; +} + +void PathSampler::onPath(const Path::SharedPtr path_ptr) +{ + time_keeper_ptr_->init(); + time_keeper_ptr_->tic(__func__); + + // check if data is ready and valid + if (!isDataReady(*path_ptr, *get_clock())) { + return; + } + + // 0. return if path is backward + // TODO(Maxime): support backward path + if (!motion_utils::isDrivingForward(path_ptr->points)) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Backward path is NOT supported. Just converting path to trajectory"); + } + // 1. create planner data + const auto planner_data = createPlannerData(*path_ptr); + // 2. generate trajectory + const auto generated_traj_points = generateTrajectory(planner_data); + // 3. extend trajectory to connect the optimized trajectory and the following path smoothly + if (!generated_traj_points.empty()) { + auto full_traj_points = extendTrajectory(planner_data.traj_points, generated_traj_points); + const auto output_traj_msg = + trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + traj_pub_->publish(output_traj_msg); + } else { + auto stopping_traj = trajectory_utils::convertToTrajectoryPoints(planner_data.traj_points); + for (auto & p : stopping_traj) p.longitudinal_velocity_mps = 0.0; + const auto output_traj_msg = + trajectory_utils::createTrajectory(path_ptr->header, stopping_traj); + traj_pub_->publish(output_traj_msg); + } + + time_keeper_ptr_->toc(__func__, ""); + *time_keeper_ptr_ << "========================================"; + time_keeper_ptr_->endLine(); + + // publish calculation_time + // NOTE: This function must be called after measuring onPath calculation time + const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); + debug_calculation_time_pub_->publish(calculation_time_msg); +} + +bool PathSampler::isDataReady(const Path & path, rclcpp::Clock clock) const +{ + if (!ego_state_ptr_) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); + return false; + } + + if (path.points.size() < 2) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); + return false; + } + + if (path.left_bound.empty() || path.right_bound.empty()) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), clock, 5000, "Left or right bound in path is empty."); + return false; + } + + return true; +} + +PlannerData PathSampler::createPlannerData(const Path & path) const +{ + // create planner data + PlannerData planner_data; + planner_data.header = path.header; + planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); + planner_data.left_bound = path.left_bound; + planner_data.right_bound = path.right_bound; + planner_data.ego_pose = ego_state_ptr_->pose.pose; + planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; + return planner_data; +} + +std::vector PathSampler::generateTrajectory(const PlannerData & planner_data) +{ + time_keeper_ptr_->tic(__func__); + + const auto & input_traj_points = planner_data.traj_points; + + auto generated_traj_points = generatePath(planner_data); + + applyInputVelocity(generated_traj_points, input_traj_points, planner_data.ego_pose); + publishDebugMarker(generated_traj_points); + + time_keeper_ptr_->toc(__func__, " "); + return generated_traj_points; +} + +std::vector PathSampler::generatePath(const PlannerData & planner_data) +{ + std::vector trajectory; + time_keeper_ptr_->tic(__func__); + const auto & p = planner_data; + + const auto path_spline = preparePathSpline(p.traj_points, params_.preprocessing.smooth_reference); + sampler_common::State current_state; + current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; + current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); + + const auto planning_state = getPlanningState(current_state, path_spline); + prepareConstraints(params_.constraints, *in_objects_ptr_, p.left_bound, p.right_bound); + + auto candidate_paths = generateCandidatePaths(planning_state, path_spline, 0.0, params_); + if (prev_path_ && prev_path_->lengths.size() > 1) { + // Update previous path + constexpr auto max_deviation = 2.0; // [m] TODO(Maxime): param + const auto closest_iter = std::min_element( + prev_path_->points.begin(), prev_path_->points.end() - 1, + [&](const auto & p1, const auto & p2) { + return boost::geometry::distance(p1, current_state.pose) <= + boost::geometry::distance(p2, current_state.pose); + }); + if ( + closest_iter != prev_path_->points.end() && + boost::geometry::distance(*closest_iter, current_state.pose) <= max_deviation) { + const auto current_idx = std::distance(prev_path_->points.begin(), closest_iter); + const auto length_offset = prev_path_->lengths[current_idx]; + for (auto & l : prev_path_->lengths) l -= length_offset; + constexpr auto behind_dist = -5.0; // [m] TODO(Maxime): param + auto behind_idx = current_idx; + while (behind_idx > 0 && prev_path_->lengths[behind_idx] > behind_dist) --behind_idx; + *prev_path_ = *prev_path_->subset(behind_idx, prev_path_->points.size()); + + // Use previous path for replanning + const auto prev_path_length = prev_path_->lengths.back(); + const auto reuse_step = prev_path_length / params_.sampling.previous_path_reuse_points_nb; + for (double reuse_length = reuse_step; reuse_length <= prev_path_length; + reuse_length += reuse_step) { + sampler_common::State reuse_state; + size_t reuse_idx = 0; + for (reuse_idx = 0; reuse_idx + 1 < prev_path_->lengths.size() && + prev_path_->lengths[reuse_idx] < reuse_length; + ++reuse_idx) + ; + if (reuse_idx == 0UL) continue; + const auto reused_path = *prev_path_->subset(0UL, reuse_idx); + reuse_state.curvature = reused_path.curvatures.back(); + reuse_state.pose = reused_path.points.back(); + reuse_state.heading = reused_path.yaws.back(); + reuse_state.frenet = path_spline.frenet(reuse_state.pose); + auto paths = generateCandidatePaths(reuse_state, path_spline, reuse_length, params_); + for (auto & p : paths) candidate_paths.push_back(reused_path.extend(p)); + } + } else { + resetPreviousData(); + } + } + debug_data_.footprints.clear(); + for (auto & path : candidate_paths) { + const auto footprint = + sampler_common::constraints::checkHardConstraints(path, params_.constraints); + debug_data_.footprints.push_back(footprint); + sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); + } + const auto best_path_idx = [](const auto & paths) { + auto min_cost = std::numeric_limits::max(); + size_t best_path_idx = 0; + for (auto i = 0LU; i < paths.size(); ++i) { + if (paths[i].constraint_results.isValid() && paths[i].cost < min_cost) { + best_path_idx = i; + min_cost = paths[i].cost; + } + } + return min_cost < std::numeric_limits::max() ? std::optional(best_path_idx) + : std::nullopt; + }; + const auto selected_path_idx = best_path_idx(candidate_paths); + if (selected_path_idx) { + const auto & selected_path = candidate_paths[*selected_path_idx]; + trajectory = trajectory_utils::convertToTrajectoryPoints(selected_path); + prev_path_ = selected_path; + } else { + RCLCPP_WARN( + get_logger(), "No valid path found (out of %lu) outputing %s\n", candidate_paths.size(), + prev_path_ ? "previous path" : "stopping path"); + int coll = 0; + int da = 0; + int k = 0; + for (const auto & p : candidate_paths) { + coll += static_cast(!p.constraint_results.collision); + da += static_cast(!p.constraint_results.drivable_area); + k += static_cast(!p.constraint_results.curvature); + } + RCLCPP_WARN(get_logger(), "\tInvalid coll/da/k = %d/%d/%d\n", coll, da, k); + if (prev_path_) trajectory = trajectory_utils::convertToTrajectoryPoints(*prev_path_); + } + time_keeper_ptr_->toc(__func__, " "); + debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size(); + debug_data_.sampled_candidates = candidate_paths; + debug_data_.obstacles = params_.constraints.obstacle_polygons; + return trajectory; +} + +void PathSampler::applyInputVelocity( + std::vector & output_traj_points, + const std::vector & input_traj_points, + const geometry_msgs::msg::Pose & ego_pose) const +{ + if (output_traj_points.empty()) return; + time_keeper_ptr_->tic(__func__); + + // crop forward for faster calculation + const auto forward_cropped_input_traj_points = [&]() { + const double generated_traj_length = motion_utils::calcArcLength(output_traj_points); + constexpr double margin_traj_length = 10.0; + + const size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); + return motion_utils::cropForwardPoints( + input_traj_points, ego_pose.position, ego_seg_idx, + generated_traj_length + margin_traj_length); + }(); + + // update velocity + size_t input_traj_start_idx = 0; + for (size_t i = 0; i < output_traj_points.size(); i++) { + // crop backward for efficient calculation + const auto cropped_input_traj_points = std::vector{ + forward_cropped_input_traj_points.begin() + input_traj_start_idx, + forward_cropped_input_traj_points.end()}; + + const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( + cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); + input_traj_start_idx = nearest_seg_idx; + + // calculate velocity with zero order hold + const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; + output_traj_points.at(i).longitudinal_velocity_mps = velocity; + } + + // insert stop point explicitly + const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + if (stop_idx) { + const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_); + + // calculate and insert stop pose on output trajectory + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); + } + + time_keeper_ptr_->toc(__func__, " "); +} + +void PathSampler::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const +{ + time_keeper_ptr_->tic(__func__); + + const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); + + virtual_wall_pub_->publish(virtual_wall_marker); + time_keeper_ptr_->toc(__func__, " "); +} + +void PathSampler::publishDebugMarker(const std::vector & traj_points) const +{ + (void)traj_points; + + time_keeper_ptr_->tic(__func__); + + // debug marker + time_keeper_ptr_->tic("getDebugMarker"); + // const auto markers = getDebugMarker(debug_data_); + visualization_msgs::msg::MarkerArray markers; + if (debug_markers_pub_->get_subscription_count() > 0LU) { + visualization_msgs::msg::Marker m; + m.header.frame_id = "map"; + m.header.stamp = this->now(); + m.action = m.ADD; + m.id = 0UL; + m.type = m.LINE_STRIP; + m.color.a = 1.0; + m.scale.x = 0.02; + m.ns = "candidates"; + for (const auto & c : debug_data_.sampled_candidates) { + m.points.clear(); + for (const auto & p : c.points) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + if (c.constraint_results.isValid()) { + m.color.g = 1.0; + m.color.r = 0.0; + } else { + m.color.r = 1.0; + m.color.g = 0.0; + } + markers.markers.push_back(m); + ++m.id; + } + if (prev_path_) { + m.ns = "previous_path"; + m.id = 0UL; + m.points.clear(); + for (const auto & p : prev_path_->points) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + m.color.r = 0.0; + m.color.g = 0.0; + m.color.b = 1.0; + markers.markers.push_back(m); + } + m.ns = "footprint"; + m.id = 0UL; + m.type = m.POINTS; + m.points.clear(); + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 1.0; + m.scale.y = 0.02; + if (!debug_data_.footprints.empty()) { + m.action = m.ADD; + for (const auto & p : + debug_data_.footprints[std::min(debug_id_, debug_data_.footprints.size())]) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + } else { + m.action = m.DELETE; + } + m.ns = "debug_path"; + m.id = 0UL; + m.type = m.POINTS; + m.points.clear(); + m.color.g = 1.0; + m.color.b = 0.0; + m.scale.y = 0.04; + if (!debug_data_.sampled_candidates.empty()) { + m.action = m.ADD; + for (const auto & p : + debug_data_ + .sampled_candidates[std::min(debug_id_, debug_data_.sampled_candidates.size())] + .points) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + } else { + m.action = m.DELETE; + } + markers.markers.push_back(m); + m.type = m.LINE_STRIP; + m.ns = "obstacles"; + m.id = 0UL; + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 0.0; + for (const auto & obs : debug_data_.obstacles) { + m.points.clear(); + for (const auto & p : obs.outer()) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + markers.markers.push_back(m); + ++m.id; + } + m.action = m.DELETE; + m.ns = "candidates"; + for (m.id = debug_data_.sampled_candidates.size(); + static_cast(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id) + markers.markers.push_back(m); + } + time_keeper_ptr_->toc("getDebugMarker", " "); + + time_keeper_ptr_->tic("publishDebugMarker"); + debug_markers_pub_->publish(markers); + time_keeper_ptr_->toc("publishDebugMarker", " "); + + time_keeper_ptr_->toc(__func__, " "); +} + +std::vector PathSampler::extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_traj_points) const +{ + time_keeper_ptr_->tic(__func__); + + const auto & start_pose = optimized_traj_points.front().pose; + + // prepend the generated trajectory: join it at the start with the reference path + const size_t start_traj_seg_idx = + trajectory_utils::findEgoSegmentIndex(traj_points, start_pose, ego_nearest_param_); + const auto prepended_traj_points = [&]() { + if (start_traj_seg_idx == 0UL) return traj_points; + const auto pre_traj = + std::vector(traj_points.begin(), traj_points.begin() + start_traj_seg_idx); + return concatVectors(pre_traj, optimized_traj_points); + }(); + + // expand the generated trajectory: join it at the end with the reference path + constexpr double joint_traj_length_for_smoothing = 5.0; + const auto & end_pose = prepended_traj_points.back().pose; + const size_t end_traj_seg_idx = + trajectory_utils::findEgoSegmentIndex(traj_points, end_pose, ego_nearest_param_); + const auto end_upto_traj_point_idx = trajectory_utils::getPointIndexAfter( + traj_points, end_pose.position, end_traj_seg_idx, joint_traj_length_for_smoothing); + + // calculate full trajectory points + const auto full_traj_points = [&]() { + if (!end_upto_traj_point_idx) { + return prepended_traj_points; + } + const auto extended_traj_points = std::vector{ + traj_points.begin() + *end_upto_traj_point_idx, traj_points.end()}; + return concatVectors(prepended_traj_points, extended_traj_points); + }(); + + // resample trajectory points + auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints( + full_traj_points, traj_param_.output_delta_arc_length); + + // update velocity on joint + for (size_t i = end_traj_seg_idx + 1; i <= end_upto_traj_point_idx; ++i) { + if (hasZeroVelocity(traj_points.at(i))) { + if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { + // Here is when current point is 0 velocity, but previous point is not 0 velocity. + const auto & input_stop_pose = traj_points.at(i).pose; + const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( + resampled_traj_points, input_stop_pose, ego_nearest_param_); + + // calculate and insert stop pose on output trajectory + trajectory_utils::insertStopPoint(resampled_traj_points, input_stop_pose, stop_seg_idx); + } + } + } + time_keeper_ptr_->toc(__func__, " "); + return resampled_traj_points; +} +} // namespace path_sampler + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(path_sampler::PathSampler) diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp new file mode 100644 index 0000000000000..b574f87e319fb --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp @@ -0,0 +1,126 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "path_sampler/path_generation.hpp" + +#include "sampler_common/structures.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace path_sampler +{ +std::vector generateCandidatePaths( + const sampler_common::State & initial_state, + const sampler_common::transform::Spline2D & path_spline, const double base_length, + const Parameters & params) +{ + std::vector paths; + const auto move_to_paths = [&](auto & paths_to_move) { + paths.insert( + paths.end(), std::make_move_iterator(paths_to_move.begin()), + std::make_move_iterator(paths_to_move.end())); + }; + + if (params.sampling.enable_frenet) { + auto frenet_paths = generateFrenetPaths(initial_state, base_length, path_spline, params); + move_to_paths(frenet_paths); + } + if (params.sampling.enable_bezier) { + const auto bezier_paths = generateBezierPaths(initial_state, base_length, path_spline, params); + move_to_paths(bezier_paths); + } + return paths; +} +std::vector generateBezierPaths( + const sampler_common::State & initial_state, const double base_length, + const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +{ + const auto initial_s = path_spline.frenet(initial_state.pose).s; + const auto max_s = path_spline.lastS(); + std::vector bezier_paths; + for (const auto target_length : params.sampling.target_lengths) { + if (target_length <= base_length) continue; + const auto target_s = std::min(max_s, initial_s + target_length - base_length); + if (target_s >= max_s) break; + sampler_common::State target_state{}; + target_state.pose = path_spline.cartesian({target_s, 0}); + target_state.curvature = path_spline.curvature(target_s); + target_state.heading = path_spline.yaw(target_s); + const auto beziers = + bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); + + const auto step = std::min(0.1, params.sampling.resolution / target_length); + for (const auto & bezier : beziers) { + sampler_common::Path path; + path.lengths.push_back(0.0); + for (double t = 0.0; t <= 1.0; t += step) { + const auto x_y = bezier.valueM(t); + path.points.emplace_back(x_y[0], x_y[1]); + path.yaws.emplace_back(bezier.heading(t)); + path.curvatures.push_back(bezier.curvature(t)); + } + for (size_t i = 0; i + 1 < path.points.size(); ++i) { + path.lengths.push_back( + path.lengths.back() + std::hypot( + path.points[i + 1].x() - path.points[i].x(), + path.points[i + 1].y() - path.points[i].y())); + } + bezier_paths.push_back(path); + } + } + return bezier_paths; +} + +std::vector generateFrenetPaths( + const sampler_common::State & initial_state, const double base_length, + const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +{ + const auto sampling_parameters = + prepareSamplingParameters(initial_state, base_length, path_spline, params); + + frenet_planner::FrenetState initial_frenet_state; + initial_frenet_state.position = path_spline.frenet(initial_state.pose); + const auto s = initial_frenet_state.position.s; + const auto d = initial_frenet_state.position.d; + // Calculate Velocity and acceleration parametrized over arc length + // From appendix I of Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame + const auto frenet_yaw = initial_state.heading - path_spline.yaw(s); + const auto path_curv = path_spline.curvature(s); + const auto delta_s = 0.001; + initial_frenet_state.lateral_velocity = (1 - path_curv * d) * std::tan(frenet_yaw); + const auto path_curv_deriv = (path_spline.curvature(s + delta_s) - path_curv) / delta_s; + const auto cos_yaw = std::cos(frenet_yaw); + if (cos_yaw == 0.0) { + initial_frenet_state.lateral_acceleration = 0.0; + } else { + initial_frenet_state.lateral_acceleration = + -(path_curv_deriv * d + path_curv * initial_frenet_state.lateral_velocity) * + std::tan(frenet_yaw) + + ((1 - path_curv * d) / (cos_yaw * cos_yaw)) * + (initial_state.curvature * ((1 - path_curv * d) / cos_yaw) - path_curv); + } + return frenet_planner::generatePaths(path_spline, initial_frenet_state, sampling_parameters); +} +} // namespace path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp new file mode 100644 index 0000000000000..25677a5b2d29b --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp @@ -0,0 +1,153 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "path_sampler/prepare_inputs.hpp" + +#include "frenet_planner/structures.hpp" +#include "path_sampler/utils/geometry_utils.hpp" +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace path_sampler +{ + +void prepareConstraints( + sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + const std::vector & left_bound, + const std::vector & right_bound) +{ + constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + for (const auto & o : predicted_objects.objects) + if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param + constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o)); + constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented + + // TODO(Maxime): directly use lines instead of polygon + sampler_common::Polygon2d drivable_area_polygon; + for (const auto & p : right_bound) drivable_area_polygon.outer().emplace_back(p.x, p.y); + for (auto it = left_bound.rbegin(); it != left_bound.rend(); ++it) + drivable_area_polygon.outer().emplace_back(it->x, it->y); + drivable_area_polygon.outer().push_back(drivable_area_polygon.outer().front()); + constraints.drivable_polygons = {drivable_area_polygon}; +} + +frenet_planner::SamplingParameters prepareSamplingParameters( + const sampler_common::State & initial_state, const double base_length, + const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +{ + // calculate target lateral positions + std::vector target_lateral_positions; + if (params.sampling.nb_target_lateral_positions > 1) { + target_lateral_positions = {0.0, initial_state.frenet.d}; + double min_d = 0.0; + double max_d = 0.0; + double min_d_s = std::numeric_limits::max(); + double max_d_s = std::numeric_limits::max(); + for (const auto & drivable_poly : params.constraints.drivable_polygons) { + for (const auto & p : drivable_poly.outer()) { + const auto frenet_coordinates = path_spline.frenet(p); + const auto d_s = std::abs(frenet_coordinates.s - initial_state.frenet.s); + if (d_s < min_d_s && frenet_coordinates.d < 0.0) { + min_d_s = d_s; + min_d = frenet_coordinates.d; + } + if (d_s < max_d_s && frenet_coordinates.d > 0.0) { + max_d_s = d_s; + max_d = frenet_coordinates.d; + } + } + } + min_d += params.constraints.ego_width / 2.0; + max_d -= params.constraints.ego_width / 2.0; + if (min_d < max_d) { + for (auto r = 0.0; r <= 1.0; r += 1.0 / (params.sampling.nb_target_lateral_positions - 1)) + target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r)); + } + } else { + target_lateral_positions = params.sampling.target_lateral_positions; + } + frenet_planner::SamplingParameters sampling_parameters; + sampling_parameters.resolution = params.sampling.resolution; + const auto max_s = path_spline.lastS(); + frenet_planner::SamplingParameter p; + for (const auto target_length : params.sampling.target_lengths) { + p.target_state.position.s = std::min( + max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length - base_length)); + for (const auto target_lateral_position : target_lateral_positions) { + p.target_state.position.d = target_lateral_position; + for (const auto target_lat_vel : params.sampling.frenet.target_lateral_velocities) { + p.target_state.lateral_velocity = target_lat_vel; + for (const auto target_lat_acc : params.sampling.frenet.target_lateral_accelerations) { + p.target_state.lateral_acceleration = target_lat_acc; + sampling_parameters.parameters.push_back(p); + } + } + } + if (p.target_state.position.s == max_s) break; + } + return sampling_parameters; +} + +sampler_common::transform::Spline2D preparePathSpline( + const std::vector & path, const bool smooth_path) +{ + std::vector x; + std::vector y; + if (smooth_path) { + // TODO(Maxime CLEMENT): this version using Eigen::Spline is unreliable and sometimes crashes + constexpr auto spline_dim = 3; + Eigen::MatrixXd control_points(path.size(), 2); + for (auto i = 0lu; i < path.size(); ++i) { + const auto & point = path[i]; + control_points(i, 0) = point.pose.position.x; + control_points(i, 1) = point.pose.position.y; + } + control_points.transposeInPlace(); + const auto nb_knots = path.size() + spline_dim + 3; + Eigen::RowVectorXd knots(nb_knots); + constexpr auto repeat_endknots = 3lu; + const auto knot_step = 1.0 / static_cast(nb_knots - 2 * repeat_endknots); + auto i = 0lu; + for (; i < repeat_endknots; ++i) knots[i] = 0.0; + for (; i < nb_knots - repeat_endknots; ++i) knots[i] = knots[i - 1] + knot_step; + for (; i < nb_knots; ++i) knots[i] = 1.0; + const auto spline = Eigen::Spline(knots, control_points); + x.reserve(path.size()); + y.reserve(path.size()); + const auto t_step = 1 / static_cast(path.size()); + for (auto t = 0.0; t < 1.0; t += t_step) { + const auto p = spline(t); + x.push_back(p.x()); + y.push_back(p.y()); + } + } else { + x.reserve(path.size()); + y.reserve(path.size()); + for (const auto & point : path) { + x.push_back(point.pose.position.x); + y.push_back(point.pose.position.y); + } + } + return {x, y}; +} +} // namespace path_sampler 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 new file mode 100644 index 0000000000000..ec41f3032e150 --- /dev/null +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -0,0 +1,118 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "path_sampler/utils/trajectory_utils.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "path_sampler/utils/geometry_utils.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include +#include +#include + +namespace path_sampler +{ +namespace trajectory_utils +{ +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold) +{ + if (traj_points.empty()) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + return; + } + + const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; + + const double dist = + tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double norm_diff_yaw = [&]() { + const double diff_yaw = + tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); + return tier4_autoware_utils::normalizeRadian(diff_yaw); + }(); + if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + } +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points) +{ + auto traj = motion_utils::convertToTrajectory(traj_points); + traj.header = header; + + return traj; +} + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval) +{ + constexpr bool enable_resampling_stop_point = true; + + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory( + traj, interval, false, true, true, enable_resampling_stop_point); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +// NOTE: stop point will not be resampled +std::vector resampleTrajectoryPointsWithoutStopPoint( + const std::vector traj_points, const double interval) +{ + constexpr bool enable_resampling_stop_point = false; + + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory( + traj, interval, false, true, true, enable_resampling_stop_point); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +void insertStopPoint( + std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, + const size_t stop_seg_idx) +{ + const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + traj_points, stop_seg_idx, input_stop_pose.position); + + const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); + + if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { + traj_points.at(stop_seg_idx).longitudinal_velocity_mps = 0.0; + return; + } + if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx + 1), stop_pose)) { + traj_points.at(stop_seg_idx + 1).longitudinal_velocity_mps = 0.0; + return; + } + + TrajectoryPoint additional_traj_point; + additional_traj_point.pose = stop_pose; + additional_traj_point.longitudinal_velocity_mps = 0.0; + + traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); +} +} // namespace trajectory_utils +} // namespace path_sampler diff --git a/planning/sampling_based_planner/sampler_common/CMakeLists.txt b/planning/sampling_based_planner/sampler_common/CMakeLists.txt new file mode 100644 index 0000000000000..5e669a705201e --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) +project(sampler_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED) + +ament_auto_add_library(sampler_common SHARED + DIRECTORY src/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_sampler_common + test/test_transform.cpp + test/test_structures.cpp + ) + + target_link_libraries(test_sampler_common + sampler_common + ) +endif() + +ament_auto_package() diff --git a/planning/sampling_based_planner/sampler_common/README.md b/planning/sampling_based_planner/sampler_common/README.md new file mode 100644 index 0000000000000..4bae0bf5d6d7d --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/README.md @@ -0,0 +1,6 @@ +# Sampler Common + +Common functions for sampling based planners. +This includes classes for representing paths and trajectories, +hard and soft constraints, +conversion between cartesian and frenet frames, ... diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp new file mode 100644 index 0000000000000..061f922001a0f --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ + +#include "sampler_common/structures.hpp" + +namespace sampler_common::constraints +{ +/// @brief Calculate the footprint of the given path +/// @param path sequence of pose used to build the footprint +/// @param constraints input constraint object containing vehicle footprint offsets +/// @return the polygon footprint of the path +MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints); +} // namespace sampler_common::constraints + +#endif // SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp new file mode 100644 index 0000000000000..475aae4aeea18 --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp @@ -0,0 +1,26 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ + +#include "sampler_common/structures.hpp" + +namespace sampler_common::constraints +{ +/// @brief Check if the path satisfies the hard constraints +MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints); +} // namespace sampler_common::constraints + +#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp new file mode 100644 index 0000000000000..ed04d8104a904 --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ + +#include "sampler_common/structures.hpp" + +#include + +#include + +namespace sampler_common::constraints +{ +struct MapConstraints +{ + MapConstraints( + const Point2d & ego_pose, const lanelet::LaneletMapConstPtr & map_ptr, + const std::vector route_ids) + { + } + /// @brief check the given path and return the corresponding cost + /// \return cost of the path. If negative then the path is invalid. + double check(const Path & path) + { + double cost = 0.0; + for (const auto & p : path.points) { + bool drivable = false; + for (size_t i = 0; i < drivable_polygons.size(); ++i) { + if (boost::geometry::within(p, drivable_polygons[i])) { + drivable = true; + cost += polygon_costs[i]; + break; + } + } + if (!drivable) { + cost = -1; + break; + } + } + return cost; + } + + MultiPolygon2d drivable_polygons; + std::vector polygon_costs; +}; +} // namespace sampler_common::constraints + +#endif // SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp new file mode 100644 index 0000000000000..9fdb9fe137e49 --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ + +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +namespace sampler_common::constraints +{ +/// @brief calculate the curvature cost of the given path +void calculateCurvatureCost(Path & path, const Constraints & constraints); +/// @brief calculate the length cost of the given path +void calculateLengthCost(Path & path, const Constraints & constraints); +/// @brief calculate the lateral deviation cost at the end of the given path +void calculateLateralDeviationCost( + Path & path, const Constraints & constraints, const transform::Spline2D & reference); +/// @brief calculate the overall cost of the given path +void calculateCost( + Path & path, const Constraints & constraints, const transform::Spline2D & reference); +} // namespace sampler_common::constraints + +#endif // SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp new file mode 100644 index 0000000000000..54e8c457034a9 --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp @@ -0,0 +1,352 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SAMPLER_COMMON__STRUCTURES_HPP_ +#define SAMPLER_COMMON__STRUCTURES_HPP_ + +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace sampler_common +{ +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +/// @brief data about constraint check results of a given path +struct ConstraintResults +{ + bool collision = true; + bool curvature = true; + bool drivable_area = true; + + [[nodiscard]] bool isValid() const { return collision && curvature && drivable_area; } + + void clear() { collision = curvature = drivable_area = true; } +}; +struct FrenetPoint +{ + FrenetPoint(double s_, double d_) : s(s_), d(d_) {} + double s = 0.0; + double d = 0.0; +}; + +struct State +{ + Point2d pose{}; + FrenetPoint frenet{0.0, 0.0}; + double curvature{}; + double heading{}; +}; + +struct Configuration : State +{ + double velocity{}; + double acceleration{}; +}; + +/// @brief Path +struct Path +{ + std::vector points{}; + std::vector curvatures{}; + std::vector yaws{}; + std::vector lengths{}; + ConstraintResults constraint_results{}; + double cost{}; + std::string tag{}; // string tag used for debugging + + Path() = default; + virtual ~Path() = default; + + virtual void clear() + { + points.clear(); + curvatures.clear(); + yaws.clear(); + lengths.clear(); + constraint_results.clear(); + tag = ""; + cost = 0.0; + } + + virtual void reserve(const size_t size) + { + points.reserve(size); + curvatures.reserve(size); + yaws.reserve(size); + lengths.reserve(size); + } + + [[nodiscard]] Path extend(const Path & path) const + { + Path extended_path; + auto offset = 0l; + auto length_offset = 0.0; + if (!points.empty() && !path.points.empty()) { + if ( + points.back().x() == path.points.front().x() && + points.back().y() == path.points.front().y()) { + offset = 1l; + } + length_offset = std::hypot( + path.points[offset].x() - points.back().x(), path.points[offset].y() - points.back().y()); + } + const auto ext = [&](auto & dest, const auto & first, const auto & second) { + dest.insert(dest.end(), first.begin(), first.end()); + dest.insert(dest.end(), std::next(second.begin(), offset), second.end()); + }; + ext(extended_path.points, points, path.points); + ext(extended_path.curvatures, curvatures, path.curvatures); + ext(extended_path.yaws, yaws, path.yaws); + extended_path.lengths.insert(extended_path.lengths.end(), lengths.begin(), lengths.end()); + const auto last_base_length = lengths.empty() ? 0.0 : lengths.back() + length_offset; + for (size_t i = offset; i < path.lengths.size(); ++i) + extended_path.lengths.push_back(last_base_length + path.lengths[i]); + extended_path.cost = path.cost; + extended_path.constraint_results = path.constraint_results; + extended_path.tag = path.tag; + return extended_path; + } + + // Return a pointer to allow overriding classes to return the appropriate type + // Without pointer we are stuck with returning a Path + [[nodiscard]] virtual Path * subset(const size_t from_idx, const size_t to_idx) const + { + auto * subpath = new Path(); + subpath->reserve(to_idx - from_idx); + + const auto copy_subset = [&](const auto & from, auto & to) { + to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); + }; + copy_subset(points, subpath->points); + copy_subset(curvatures, subpath->curvatures); + copy_subset(yaws, subpath->yaws); + copy_subset(lengths, subpath->lengths); + return subpath; + }; +}; + +struct Trajectory : Path +{ + std::vector longitudinal_velocities{}; + std::vector longitudinal_accelerations{}; + std::vector lateral_velocities{}; + std::vector lateral_accelerations{}; + std::vector jerks{}; + std::vector times{}; + + Trajectory() = default; + ~Trajectory() override = default; + explicit Trajectory(const Path & path) : Path(path) {} + + void clear() override + { + Path::clear(); + longitudinal_velocities.clear(); + longitudinal_accelerations.clear(); + lateral_velocities.clear(); + lateral_accelerations.clear(); + jerks.clear(); + times.clear(); + } + + void reserve(const size_t size) override + { + Path::reserve(size); + longitudinal_velocities.reserve(size); + longitudinal_accelerations.reserve(size); + lateral_velocities.reserve(size); + lateral_accelerations.reserve(size); + jerks.reserve(size); + times.reserve(size); + } + + [[nodiscard]] Trajectory extend(const Trajectory & traj) const + { + Trajectory extended_traj(Path::extend(traj)); + auto offset = 0l; + auto time_offset = 0.0; + // TODO(Maxime): remove these checks + if (!points.empty() && !traj.points.empty()) { + if ( + points.back().x() == traj.points.front().x() && + points.back().y() == traj.points.front().y()) + offset = 1l; + const auto ds = std::hypot( + traj.points[offset].x() - points.back().x(), traj.points[offset].y() - points.back().y()); + const auto v = std::max( + std::max(longitudinal_velocities.back(), traj.longitudinal_velocities[offset]), 0.1); + time_offset = std::abs(ds / v); + } + const auto ext = [&](auto & dest, const auto & first, const auto & second) { + dest.insert(dest.end(), first.begin(), first.end()); + dest.insert(dest.end(), std::next(second.begin(), offset), second.end()); + }; + ext( + extended_traj.longitudinal_velocities, longitudinal_velocities, traj.longitudinal_velocities); + ext( + extended_traj.longitudinal_accelerations, longitudinal_accelerations, + traj.longitudinal_accelerations); + ext(extended_traj.lateral_velocities, lateral_velocities, traj.lateral_velocities); + ext(extended_traj.lateral_accelerations, lateral_accelerations, traj.lateral_accelerations); + ext(extended_traj.jerks, jerks, traj.jerks); + extended_traj.times.insert(extended_traj.times.end(), times.begin(), times.end()); + const auto last_base_time = times.empty() ? 0.0 : times.back() + time_offset; + for (size_t i = offset; i < traj.times.size(); ++i) + extended_traj.times.push_back(last_base_time + traj.times[i]); + return extended_traj; + } + + [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override + { + auto * subtraj = new Trajectory(*Path::subset(from_idx, to_idx)); + + const auto copy_subset = [&](const auto & from, auto & to) { + to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); + }; + + copy_subset(longitudinal_velocities, subtraj->longitudinal_velocities); + copy_subset(longitudinal_accelerations, subtraj->longitudinal_accelerations); + copy_subset(lateral_velocities, subtraj->lateral_velocities); + copy_subset(lateral_accelerations, subtraj->lateral_accelerations); + copy_subset(jerks, subtraj->jerks); + copy_subset(times, subtraj->times); + return subtraj; + } + + [[nodiscard]] Trajectory resample(const double fixed_interval) const + { + Trajectory t; + if (lengths.size() < 2 || fixed_interval <= 0.0) return *this; + + const auto new_size = + static_cast((lengths.back() - lengths.front()) / fixed_interval) + 1; + t.times.reserve(new_size); + t.lengths.reserve(new_size); + for (auto i = 0lu; i < new_size; ++i) + t.lengths.push_back(lengths.front() + static_cast(i) * fixed_interval); + t.times = interpolation::lerp(lengths, times, t.lengths); + std::vector xs; + std::vector ys; + xs.reserve(points.size()); + ys.reserve(points.size()); + for (const auto & p : points) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + const auto new_xs = interpolation::lerp(times, xs, t.times); + const auto new_ys = interpolation::lerp(times, ys, t.times); + for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]); + t.curvatures = interpolation::lerp(times, curvatures, t.times); + t.jerks = interpolation::lerp(times, jerks, t.times); + t.yaws = interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times); + t.constraint_results = constraint_results; + t.cost = cost; + return t; + } + + [[nodiscard]] Trajectory resampleTimeFromZero(const double fixed_interval) const + { + Trajectory t; + if (times.size() < 2 || times.back() < 0.0 || fixed_interval <= 0.0) return *this; + + const auto min_time = 0; + const auto max_time = times.back(); + const auto new_size = static_cast((max_time - min_time) / fixed_interval) + 1; + t.times.reserve(new_size); + t.lengths.reserve(new_size); + for (auto i = 0lu; i < new_size; ++i) + t.times.push_back(static_cast(i) * fixed_interval); + t.lengths = interpolation::lerp(times, lengths, t.times); + std::vector xs; + std::vector ys; + xs.reserve(points.size()); + ys.reserve(points.size()); + for (const auto & p : points) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + const auto new_xs = interpolation::lerp(times, xs, t.times); + const auto new_ys = interpolation::lerp(times, ys, t.times); + for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]); + t.curvatures = interpolation::lerp(times, curvatures, t.times); + t.jerks = interpolation::lerp(times, jerks, t.times); + t.yaws = interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times); + t.constraint_results = constraint_results; + t.cost = cost; + return t; + } +}; + +struct DynamicObstacle +{ + std::vector footprint_per_time; + double time_step; // [s] time step between each footprint +}; + +struct Constraints +{ + struct + { + double lateral_deviation_weight; + double length_weight; + double curvature_weight; + } soft{}; + struct + { + double min_curvature; + double max_curvature; + } hard{}; + LinearRing2d ego_footprint; + double ego_width; + double ego_length; + MultiPolygon2d obstacle_polygons; + MultiPolygon2d drivable_polygons; + std::vector dynamic_obstacles; +}; + +struct ReusableTrajectory +{ + Trajectory trajectory; // base trajectory + Configuration planning_configuration; // planning configuration at the end of the trajectory +}; + +} // namespace sampler_common + +#endif // SAMPLER_COMMON__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp new file mode 100644 index 0000000000000..ce66141832dbf --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp @@ -0,0 +1,85 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ + +#include "sampler_common/structures.hpp" + +#include + +namespace sampler_common::transform +{ +using sampler_common::FrenetPoint; + +class Spline +{ + std::vector a_{}; + std::vector b_{}; + std::vector c_{}; + std::vector d_{}; + std::vector h_{}; + +public: + Spline() = default; + Spline(const std::vector & base_index, const std::vector & base_value); + explicit Spline(const std::vector & points); + bool interpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value); + [[nodiscard]] double value(const double query, const std::vector & base_index) const; + [[nodiscard]] double velocity(const double query, const std::vector & base_index) const; + [[nodiscard]] double acceleration( + const double query, const std::vector & base_index) const; + +private: + void generateSpline( + const std::vector & base_index, const std::vector & base_value); + [[nodiscard]] static bool isIncrease(const std::vector & x); + [[nodiscard]] static bool isValidInput( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index); + [[nodiscard]] std::vector solveLinearSystem( + const double omega, const size_t max_iter) const; + [[nodiscard]] static bool isConvergeL1( + const std::vector & r1, const std::vector & r2, const double converge_range); +}; + +class Spline2D +{ + std::vector s_{}; + Spline x_spline_{}; + Spline y_spline_{}; + + std::vector original_points_{}; + +public: + Spline2D() = default; + Spline2D(const std::vector & x, const std::vector & y); + [[nodiscard]] FrenetPoint frenet_naive(const Point2d & p, const double precision = 0.01) const; + [[nodiscard]] FrenetPoint frenet(const Point2d & p, const double precision = 0.01) const; + [[nodiscard]] Point2d cartesian(const double s) const; + [[nodiscard]] Point2d cartesian(const FrenetPoint & fp) const; + [[nodiscard]] double curvature(const double s) const; + [[nodiscard]] double yaw(const double s) const; + [[nodiscard]] double firstS() const { return s_.empty() ? 0.0 : s_.front(); } + [[nodiscard]] double lastS() const { return s_.empty() ? 0.0 : s_.back(); } + +private: + static std::vector arcLength( + const std::vector & x, const std::vector & y); +}; +} // namespace sampler_common::transform + +#endif // SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/package.xml b/planning/sampling_based_planner/sampler_common/package.xml new file mode 100644 index 0000000000000..321ca2fd43fef --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/package.xml @@ -0,0 +1,23 @@ + + + sampler_common + 0.0.1 + Common classes and functions for sampling based planners + Maxime CLEMENT + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + interpolation + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp b/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp new file mode 100644 index 0000000000000..92c8ae65267db --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp @@ -0,0 +1,53 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sampler_common/constraints/footprint.hpp" + +#include "sampler_common/structures.hpp" + +#include + +#include +#include + +#include +#include +#include + +namespace sampler_common::constraints +{ + +namespace +{ +const auto to_eigen = [](const Point2d & p) { return Eigen::Vector2d(p.x(), p.y()); }; +} // namespace + +MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints) +{ + MultiPoint2d footprint; + + footprint.reserve(path.points.size() * 4); + for (auto i = 0UL; i < path.points.size(); ++i) { + const Eigen::Vector2d p = to_eigen(path.points[i]); + const double heading = path.yaws[i]; + Eigen::Matrix2d rotation; + rotation << std::cos(heading), -std::sin(heading), std::sin(heading), std::cos(heading); + for (const auto & fp : constraints.ego_footprint) { + const Eigen::Vector2d fp_point = p + rotation * fp; + footprint.emplace_back(fp_point.x(), fp_point.y()); + } + } + return footprint; +} +} // namespace sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp b/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp new file mode 100644 index 0000000000000..40d7efa739e6b --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp @@ -0,0 +1,56 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sampler_common/constraints/hard_constraint.hpp" + +#include "sampler_common/constraints/footprint.hpp" + +#include + +#include + +namespace sampler_common::constraints +{ +bool satisfyMinMax(const std::vector & values, const double min, const double max) +{ + for (const auto value : values) { + if (value < min || value > max) return false; + } + return true; +} + +bool has_collision(const MultiPoint2d & footprint, const MultiPolygon2d & obstacles) +{ + for (const auto & o : obstacles) + for (const auto & p : footprint) + if (boost::geometry::within(p, o)) return true; + return false; +} + +MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints) +{ + const auto footprint = buildFootprintPoints(path, constraints); + if (!footprint.empty()) { + if (!boost::geometry::within(footprint, constraints.drivable_polygons)) { + path.constraint_results.drivable_area = false; + } + } + path.constraint_results.collision = !has_collision(footprint, constraints.obstacle_polygons); + if (!satisfyMinMax( + path.curvatures, constraints.hard.min_curvature, constraints.hard.max_curvature)) { + path.constraint_results.curvature = false; + } + return footprint; +} +} // namespace sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp new file mode 100644 index 0000000000000..b1d390e68c49f --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp @@ -0,0 +1,54 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sampler_common/constraints/soft_constraint.hpp" + +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +#include + +namespace sampler_common::constraints +{ +void calculateCurvatureCost(Path & path, const Constraints & constraints) +{ + double curvature_sum = 0.0; + for (const auto curvature : path.curvatures) { + curvature_sum += std::abs(curvature); + } + path.cost += + constraints.soft.curvature_weight * curvature_sum / static_cast(path.curvatures.size()); +} +void calculateLengthCost(Path & path, const Constraints & constraints) +{ + if (!path.lengths.empty()) path.cost -= constraints.soft.length_weight * path.lengths.back(); +} + +void calculateLateralDeviationCost( + Path & path, const Constraints & constraints, const transform::Spline2D & reference) +{ + const auto fp = reference.frenet(path.points.back()); + const double lateral_deviation = std::abs(fp.d); + path.cost += constraints.soft.lateral_deviation_weight * lateral_deviation; +} + +void calculateCost( + Path & path, const Constraints & constraints, const transform::Spline2D & reference) +{ + if (path.points.empty()) return; + calculateCurvatureCost(path, constraints); + calculateLengthCost(path, constraints); + calculateLateralDeviationCost(path, constraints, reference); +} +} // namespace sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp new file mode 100644 index 0000000000000..cf5f1e5478fe5 --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp @@ -0,0 +1,315 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +namespace sampler_common::transform +{ +Spline::Spline(const std::vector & base_index, const std::vector & base_value) +{ + generateSpline(base_index, base_value); +} + +Spline::Spline(const std::vector & points) +{ + std::vector xs; + std::vector ys; + xs.reserve(points.size()); + ys.reserve(points.size()); + for (const auto & p : points) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + generateSpline(xs, ys); +} + +void Spline::generateSpline( + const std::vector & base_index, const std::vector & base_value) +{ + const size_t N = base_value.size(); + + a_.clear(); + b_.clear(); + c_.clear(); + d_.clear(); + h_.clear(); + + a_ = base_value; + + for (size_t i = 0; i + 1 < N; ++i) { + h_.push_back(base_index[i + 1] - base_index[i]); + } + + c_ = solveLinearSystem(1.8, 100); + + for (size_t i = 0; i < N - 1; i++) { + d_.push_back((c_[i + 1] - c_[i]) / (3.0 * h_[i])); + b_.push_back((a_[i + 1] - a_[i]) / h_[i] - h_[i] * (2.0 * c_[i] + c_[i + 1]) / 3.0); + } + + d_.push_back(0.0); + b_.push_back(0.0); +} + +double Spline::value(const double query, const std::vector & base_index) const +{ + const auto ub = std::upper_bound(base_index.begin(), base_index.end(), query); + const size_t i = std::distance(base_index.begin(), ub) - 1; + const double ds = query - base_index[i]; + return a_[i] + (b_[i] + (c_[i] + d_[i] * ds) * ds) * ds; +} + +double Spline::velocity(const double query, const std::vector & base_index) const +{ + const auto lb = std::lower_bound(base_index.begin(), base_index.end(), query); + const size_t i = std::distance(base_index.begin(), lb) - 1; + const auto delta = query - base_index[i]; + return b_[i] + 2.0 * c_[i] * delta + 3.0 * d_[i] * delta * delta; +} + +double Spline::acceleration(const double query, const std::vector & base_index) const +{ + const auto lb = std::lower_bound(base_index.begin(), base_index.end(), query); + const size_t i = std::distance(base_index.begin(), lb) - 1; + return 2.0 * c_[i] + 6.0 * d_[i] * (query - base_index[i]); +} + +bool Spline::isIncrease(const std::vector & x) +{ + for (int i = 0; i < static_cast(x.size()) - 1; ++i) { + if (x[i] > x[i + 1]) return false; + } + return true; +} + +bool Spline::isValidInput( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index) +{ + if (base_index.empty() || base_value.empty() || return_index.empty()) { + std::cerr << "bad index : some vector is empty. base_index: " << base_index.size() + << ", base_value: " << base_value.size() << ", return_index: " << return_index.size() + << std::endl; + return false; + } + if (!isIncrease(base_index)) { + std::cerr << "bad index : base_index is not monotonically increasing. base_index = [" + << base_index.front() << ", " << base_index.back() << "]" << std::endl; + return false; + } + if (!isIncrease(return_index)) { + std::cerr << "bad index : base_index is not monotonically increasing. return_index = [" + << return_index.front() << ", " << return_index.back() << "]" << std::endl; + return false; + } + if (return_index.front() < base_index.front()) { + std::cerr << "bad index : return_index.front() < base_index.front()" << std::endl; + return false; + } + if (base_index.back() < return_index.back()) { + std::cerr << "bad index : base_index.back() < return_index.back()" << std::endl; + return false; + } + if (base_index.size() != base_value.size()) { + std::cerr << "bad index : base_index.size() != base_value.size()" << std::endl; + return false; + } + + return true; +} + +std::vector Spline::solveLinearSystem(const double omega, const size_t max_iter) const +{ + // solve A * ans = rhs by SOR method + constexpr double converge_range = 0.00001; + std::vector ans(a_.size(), 1.0); + std::vector ans_next(a_.size(), 0.0); + size_t num_iter = 0; + + while (!isConvergeL1(ans, ans_next, converge_range) && num_iter <= max_iter) { + ans = ans_next; + for (size_t i = 1; i < a_.size() - 1; ++i) { + const double rhs = 3.0 / h_[i] * (a_[i + 1] - a_[i]) - 3.0 / h_[i - 1] * (a_[i] - a_[i - 1]); + ans_next[i] += omega / (2.0 * (h_[i - 1] + h_[i])) * + (rhs - (h_[i - 1] * ans_next[i - 1] + 2.0 * (h_[i - 1] + h_[i]) * ans[i] + + h_[i] * ans[i + 1])); + } + ++num_iter; + } + + if (num_iter > max_iter) std::cerr << "[Spline::solveLinearSystem] unconverged!\n"; + return ans_next; +} + +bool Spline::isConvergeL1( + const std::vector & r1, const std::vector & r2, const double converge_range) +{ + double d = 0.0; + for (size_t i = 0; i < r1.size(); ++i) { + d += std::fabs(r1.at(i) - r2.at(i)); + } + return d < converge_range; +} + +/* + * 2D Spline + */ + +Spline2D::Spline2D(const std::vector & x, const std::vector & y) +: s_(arcLength(x, y)), x_spline_{s_, x}, y_spline_{s_, y} +{ + original_points_.reserve(x.size()); + for (size_t i = 0; i < x.size(); ++i) { + original_points_.emplace_back(x[i], y[i]); + } +} + +// @brief Calculate the distances of the points along the path +// @details approximation using linear segments +std::vector Spline2D::arcLength( + const std::vector & x, const std::vector & y) +{ + std::vector s = {0.0}; + s.reserve(x.size() - 1); + for (size_t i = 0; i < x.size() - 1; ++i) { + const double ds = std::hypot((x[i + 1] - x[i]), (y[i + 1] - y[i])); + s.push_back(s.back() + ds); + } + return s; +} + +// @brief Convert the given point to the Frenet frame of this spline +// @details sample points along the splines and return the closest one +FrenetPoint Spline2D::frenet_naive(const Point2d & p, double precision) const +{ + double closest_d = std::numeric_limits::max(); + double arc_length = std::numeric_limits::min(); + for (double s = s_.front(); s < s_.back(); s += precision) { + const double x_s = x_spline_.value(s, s_); + const double y_s = y_spline_.value(s, s_); + const double d = std::hypot(p.x() - x_s, p.y() - y_s); + if (d <= closest_d) // also accept equality to return the largest possible arc length + { + closest_d = d; + arc_length = s; + } + } + // check sign of d + const double x0 = x_spline_.value(arc_length, s_); + const double y0 = y_spline_.value(arc_length, s_); + const double x1 = x_spline_.value(arc_length + precision, s_); + const double y1 = y_spline_.value(arc_length + precision, s_); + if ((x1 - x0) * (p.y() - y0) - (y1 - y0) * (p.x() - x0) < 0) { + closest_d *= -1.0; + } + return {arc_length, closest_d}; +} + +// @brief Convert the given point to the Frenet frame of this spline +// @details find closest point in the lookup table +// @warning can fail if the original points are not smooth or if some points are very far apart +FrenetPoint Spline2D::frenet(const Point2d & p, const double precision) const +{ + const auto distance = [&](const Point2d & p2) { + return std::hypot(p.x() - p2.x(), p.y() - p2.y()); + }; + size_t min_i{}; + auto min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < original_points_.size(); ++i) { + const auto dist = distance(original_points_[i]); + if (dist <= min_dist) { + min_dist = dist; + min_i = i; + } + } + auto lb_i = min_i == 0 ? min_i : min_i - 1; + auto ub_i = min_i + 1 == original_points_.size() ? min_i : min_i + 1; + auto best_s = s_[min_i]; + // real closest s is either in interval [lb_i:min_i] or interval [min_i:ub] + // continue exploring the interval whose middle point is closest to the input point + std::vector s_interval = {s_[lb_i], {}, s_[min_i], {}, s_[ub_i]}; + std::vector d_interval = { + distance(original_points_[lb_i]), + {}, + distance(original_points_[min_i]), + {}, + distance(original_points_[ub_i])}; + while (s_interval[4] - s_interval[0] > precision) { + s_interval[1] = s_interval[0] + (s_interval[2] - s_interval[0]) / 2; + s_interval[3] = s_interval[2] + (s_interval[4] - s_interval[2]) / 2; + d_interval[1] = + distance({x_spline_.value(s_interval[1], s_), y_spline_.value(s_interval[1], s_)}); + d_interval[3] = + distance({x_spline_.value(s_interval[3], s_), y_spline_.value(s_interval[3], s_)}); + + for (auto i = 0; i < 5; ++i) { + if (d_interval[i] <= min_dist) { + min_dist = d_interval[i]; + min_i = i; + } + } + + best_s = s_interval[min_i]; + lb_i = min_i == 0 ? min_i : min_i - 1; + ub_i = min_i == 4 ? min_i : min_i + 1; + s_interval = {s_interval[lb_i], {}, s_interval[min_i], {}, s_interval[ub_i]}; + d_interval = {d_interval[lb_i], {}, d_interval[min_i], {}, d_interval[ub_i]}; + } + // check sign of d + const double x0 = x_spline_.value(best_s, s_); + const double y0 = y_spline_.value(best_s, s_); + const double x1 = x_spline_.value(best_s + precision, s_); + const double y1 = y_spline_.value(best_s + precision, s_); + if ((x1 - x0) * (p.y() - y0) - (y1 - y0) * (p.x() - x0) < 0) { + min_dist *= -1.0; + } + return {best_s, min_dist}; +} + +Point2d Spline2D::cartesian(const double s) const +{ + return {x_spline_.value(s, s_), y_spline_.value(s, s_)}; +} + +Point2d Spline2D::cartesian(const FrenetPoint & fp) const +{ + const auto heading = yaw(fp.s); + const auto x = x_spline_.value(fp.s, s_); + const auto y = y_spline_.value(fp.s, s_); + return {x + fp.d * std::cos(heading + M_PI_2), y + fp.d * std::sin(heading + M_PI_2)}; +} + +double Spline2D::curvature(const double s) const +{ + // TODO(Maxime CLEMENT) search for s in s_ here and pass index + const double x_vel = x_spline_.velocity(s, s_); + const double y_vel = y_spline_.velocity(s, s_); + const double x_acc = x_spline_.acceleration(s, s_); + const double y_acc = y_spline_.acceleration(s, s_); + return (y_acc * x_vel - x_acc * y_vel) / std::pow(x_vel * x_vel + y_vel * y_vel, 3.0 / 2.0); +} + +double Spline2D::yaw(const double s) const +{ + const double x_vel = x_spline_.velocity(s, s_); + const double y_vel = y_spline_.velocity(s, s_); + return std::atan2(y_vel, x_vel); +} + +} // namespace sampler_common::transform diff --git a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp b/planning/sampling_based_planner/sampler_common/test/test_structures.cpp new file mode 100644 index 0000000000000..006768c9325e2 --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/test/test_structures.cpp @@ -0,0 +1,161 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +TEST(Path, extendPath) +{ + using sampler_common::Path; + Path traj1; + Path traj2; + Path traj3 = traj1.extend(traj2); + EXPECT_TRUE(traj3.points.empty()); + + traj2.points = {{0, 0}, {1, 1}}; + traj3 = traj1.extend(traj2); + for (size_t i = 0; i < traj1.points.size(); ++i) { + EXPECT_EQ(traj3.points[i].x(), traj2.points[i].x()); + EXPECT_EQ(traj3.points[i].y(), traj2.points[i].y()); + } + + traj2.points = {{2, 2}, {3, 3}}; + traj3 = traj3.extend(traj2); + ASSERT_EQ(traj3.points.size(), 4ul); + for (size_t i = 0; i < traj1.points.size(); ++i) { + EXPECT_EQ(traj3.points[i].x(), i); + EXPECT_EQ(traj3.points[i].y(), i); + } +} + +TEST(Trajectory, resample) +{ + constexpr auto eps = 1e-6; + using sampler_common::Trajectory; + + Trajectory t; + t.reserve(2); + t.points = {{0, 0}, {1, 1}}; + t.yaws = {0.0, M_PI}; + t.lengths = {0.0, 1.0}; + t.times = {0.0, 1.0}; + t.jerks = {0.0, 0.0}; + t.longitudinal_velocities = {0.0, 0.5}; + t.longitudinal_accelerations = {0.0, 1.0}; + t.lateral_velocities = {0.0, 1.5}; + t.lateral_accelerations = {0.0, 3.0}; + t.curvatures = {0.0, 0.9}; + + Trajectory t2 = t.resample(0.5); + ASSERT_EQ(t2.points.size(), 3lu); + EXPECT_NEAR(t2.points[0].x(), 0.0, eps); + EXPECT_NEAR(t2.points[1].x(), 0.5, eps); + EXPECT_NEAR(t2.points[2].x(), 1.0, eps); + EXPECT_NEAR(t2.points[0].y(), 0.0, eps); + EXPECT_NEAR(t2.points[1].y(), 0.5, eps); + EXPECT_NEAR(t2.points[2].y(), 1.0, eps); + ASSERT_EQ(t2.yaws.size(), 3lu); + EXPECT_NEAR(t2.yaws[0], 0.0, eps); + EXPECT_NEAR(t2.yaws[1], M_PI_2, eps); + EXPECT_NEAR(t2.yaws[2], M_PI, eps); + ASSERT_EQ(t2.lengths.size(), 3lu); + EXPECT_NEAR(t2.lengths[0], 0.0, eps); + EXPECT_NEAR(t2.lengths[1], 0.5, eps); + EXPECT_NEAR(t2.lengths[2], 1.0, eps); + ASSERT_EQ(t2.times.size(), 3lu); + EXPECT_NEAR(t2.times[0], 0.0, eps); + EXPECT_NEAR(t2.times[1], 0.5, eps); + EXPECT_NEAR(t2.times[2], 1.0, eps); + ASSERT_EQ(t2.jerks.size(), 3lu); + EXPECT_NEAR(t2.jerks[0], 0.0, eps); + EXPECT_NEAR(t2.jerks[1], 0.0, eps); + EXPECT_NEAR(t2.jerks[2], 0.0, eps); + ASSERT_EQ(t2.longitudinal_velocities.size(), 3lu); + EXPECT_NEAR(t2.longitudinal_velocities[0], 0.0, eps); + EXPECT_NEAR(t2.longitudinal_velocities[1], 0.25, eps); + EXPECT_NEAR(t2.longitudinal_velocities[2], 0.5, eps); + ASSERT_EQ(t2.longitudinal_accelerations.size(), 3lu); + EXPECT_NEAR(t2.longitudinal_accelerations[0], 0.0, eps); + EXPECT_NEAR(t2.longitudinal_accelerations[1], 0.5, eps); + EXPECT_NEAR(t2.longitudinal_accelerations[2], 1.0, eps); + ASSERT_EQ(t2.lateral_velocities.size(), 3lu); + EXPECT_NEAR(t2.lateral_velocities[0], 0.0, eps); + EXPECT_NEAR(t2.lateral_velocities[1], 0.75, eps); + EXPECT_NEAR(t2.lateral_velocities[2], 1.5, eps); + ASSERT_EQ(t2.lateral_accelerations.size(), 3lu); + EXPECT_NEAR(t2.lateral_accelerations[0], 0.0, eps); + EXPECT_NEAR(t2.lateral_accelerations[1], 1.5, eps); + EXPECT_NEAR(t2.lateral_accelerations[2], 3.0, eps); +} + +TEST(Trajectory, resampleTime) +{ + constexpr auto eps = 1e-6; + using sampler_common::Trajectory; + + Trajectory t; + t.reserve(2); + t.points = {{0, 0}, {1, 1}}; + t.yaws = {0.0, M_PI}; + t.lengths = {0.0, 1.0}; + t.times = {0.0, 1.0}; + t.jerks = {0.0, 0.0}; + t.longitudinal_velocities = {0.0, 0.5}; + t.longitudinal_accelerations = {0.0, 1.0}; + t.lateral_velocities = {0.0, 1.5}; + t.lateral_accelerations = {0.0, 3.0}; + t.curvatures = {0.0, 0.9}; + + Trajectory t2 = t.resampleTimeFromZero(0.5); + ASSERT_EQ(t2.points.size(), 3lu); + EXPECT_NEAR(t2.points[0].x(), 0.0, eps); + EXPECT_NEAR(t2.points[1].x(), 0.5, eps); + EXPECT_NEAR(t2.points[2].x(), 1.0, eps); + EXPECT_NEAR(t2.points[0].y(), 0.0, eps); + EXPECT_NEAR(t2.points[1].y(), 0.5, eps); + EXPECT_NEAR(t2.points[2].y(), 1.0, eps); + ASSERT_EQ(t2.yaws.size(), 3lu); + EXPECT_NEAR(t2.yaws[0], 0.0, eps); + EXPECT_NEAR(t2.yaws[1], M_PI_2, eps); + EXPECT_NEAR(t2.yaws[2], M_PI, eps); + ASSERT_EQ(t2.lengths.size(), 3lu); + EXPECT_NEAR(t2.lengths[0], 0.0, eps); + EXPECT_NEAR(t2.lengths[1], 0.5, eps); + EXPECT_NEAR(t2.lengths[2], 1.0, eps); + ASSERT_EQ(t2.times.size(), 3lu); + EXPECT_NEAR(t2.times[0], 0.0, eps); + EXPECT_NEAR(t2.times[1], 0.5, eps); + EXPECT_NEAR(t2.times[2], 1.0, eps); + ASSERT_EQ(t2.jerks.size(), 3lu); + EXPECT_NEAR(t2.jerks[0], 0.0, eps); + EXPECT_NEAR(t2.jerks[1], 0.0, eps); + EXPECT_NEAR(t2.jerks[2], 0.0, eps); + ASSERT_EQ(t2.longitudinal_velocities.size(), 3lu); + EXPECT_NEAR(t2.longitudinal_velocities[0], 0.0, eps); + EXPECT_NEAR(t2.longitudinal_velocities[1], 0.25, eps); + EXPECT_NEAR(t2.longitudinal_velocities[2], 0.5, eps); + ASSERT_EQ(t2.longitudinal_accelerations.size(), 3lu); + EXPECT_NEAR(t2.longitudinal_accelerations[0], 0.0, eps); + EXPECT_NEAR(t2.longitudinal_accelerations[1], 0.5, eps); + EXPECT_NEAR(t2.longitudinal_accelerations[2], 1.0, eps); + ASSERT_EQ(t2.lateral_velocities.size(), 3lu); + EXPECT_NEAR(t2.lateral_velocities[0], 0.0, eps); + EXPECT_NEAR(t2.lateral_velocities[1], 0.75, eps); + EXPECT_NEAR(t2.lateral_velocities[2], 1.5, eps); + ASSERT_EQ(t2.lateral_accelerations.size(), 3lu); + EXPECT_NEAR(t2.lateral_accelerations[0], 0.0, eps); + EXPECT_NEAR(t2.lateral_accelerations[1], 1.5, eps); + EXPECT_NEAR(t2.lateral_accelerations[2], 3.0, eps); +} diff --git a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp b/planning/sampling_based_planner/sampler_common/test/test_transform.cpp new file mode 100644 index 0000000000000..d7998d7b8879e --- /dev/null +++ b/planning/sampling_based_planner/sampler_common/test/test_transform.cpp @@ -0,0 +1,111 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include + +constexpr auto TOL = 1E-6; // 1µm tolerance + +TEST(splineTransform, makeSpline2D) +{ + sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); + sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); +} + +TEST(splineTransform, toFrenet) +{ + sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + auto frenet = spline.frenet({0.0, 0.0}); + EXPECT_NEAR(frenet.s, 0.0, TOL); + EXPECT_NEAR(frenet.d, 0.0, TOL); + frenet = spline.frenet({1.0, 0.0}); + EXPECT_NEAR(frenet.s, 1.0, TOL); + EXPECT_NEAR(frenet.d, 0.0, TOL); + frenet = spline.frenet({2.0, 0.0}); + EXPECT_NEAR(frenet.s, 2.0, TOL); + EXPECT_NEAR(frenet.d, 0.0, TOL); + frenet = spline.frenet({1.0, 1.0}); + EXPECT_NEAR(frenet.s, 1.0, TOL); + EXPECT_NEAR(frenet.d, 1.0, TOL); + frenet = spline.frenet({1.5, -2.0}); + EXPECT_NEAR(frenet.s, 1.5, TOL); + EXPECT_NEAR(frenet.d, -2.0, TOL); + GTEST_SKIP() << "Skipping edge cases"; + // EDGE CASE before spline + frenet = spline.frenet({-1.0, 1.0}); + EXPECT_NEAR(frenet.s, -1.0, TOL); + EXPECT_NEAR(frenet.d, 1.0, TOL); + // EDGE CASE after spline + frenet = spline.frenet({3.0, -1.0}); + EXPECT_NEAR(frenet.s, 1.0, TOL); + EXPECT_NEAR(frenet.d, -1.0, TOL); + // EDGE CASE 90 deg angle + sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); + frenet = spline2.frenet({1.0, 2.0}); + EXPECT_NEAR(frenet.s, 1.0, TOL); + EXPECT_NEAR(frenet.d, 1.0, TOL); +} + +TEST(splineTransform, toCartesian) +{ + sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + auto cart = spline.cartesian({1.0, 0.0}); + EXPECT_NEAR(cart.x(), 1.0, TOL); + EXPECT_NEAR(cart.y(), 0.0, TOL); +} + +TEST(splineTransform, benchFrenet) +{ + GTEST_SKIP() << "Skipping benchmark test"; + std::random_device rd; + std::mt19937 gen(rd()); + constexpr auto precision = 1e-2; + for (auto size = 100; size <= 1000; size += 100) { + std::chrono::nanoseconds naive{0}; + std::chrono::nanoseconds lut{0}; + std::vector xs(size); + std::vector ys(size); + auto x = 0.0; + auto y = 0.0; + std::generate(xs.begin(), xs.end(), [&]() { return ++x; }); + std::generate(ys.begin(), ys.end(), [&]() { return ++y; }); + sampler_common::transform::Spline2D spline(xs, ys); + auto points_distribution = std::uniform_real_distribution(0.0, static_cast(size)); + constexpr auto nb_iter = 1e3; + for (auto iter = 0; iter < nb_iter; ++iter) { + double x = points_distribution(gen); + double y = points_distribution(gen); + auto naive_start = std::chrono::steady_clock::now(); + auto frenet_naive = spline.frenet_naive({x, y}, precision); + auto naive_end = std::chrono::steady_clock::now(); + naive += naive_end - naive_start; + auto lut_start = std::chrono::steady_clock::now(); + auto frenet_lut = spline.frenet({x, y}, precision); + auto lut_end = std::chrono::steady_clock::now(); + lut += lut_end - lut_start; + EXPECT_NEAR(frenet_naive.s, frenet_lut.s, precision); + EXPECT_NEAR(frenet_naive.d, frenet_lut.d, precision); + } + std::cout << "size = " << size << std::endl; + std::cout << "\tnaive: " << std::chrono::duration_cast(naive).count() + << "ms\n"; + std::cout << "\tlut : " << std::chrono::duration_cast(lut).count() + << "ms\n"; + } +} diff --git a/planning/scenario_selector/CMakeLists.txt b/planning/scenario_selector/CMakeLists.txt index 9ff0d09b28150..e278812cb211d 100644 --- a/planning/scenario_selector/CMakeLists.txt +++ b/planning/scenario_selector/CMakeLists.txt @@ -13,6 +13,16 @@ rclcpp_components_register_node(scenario_selector_node EXECUTABLE scenario_selector ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_${PROJECT_NAME}_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + + ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index a30893347637e..7dd9fc071976d 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -3,7 +3,7 @@ scenario_selector 0.1.0 - The scenario_selector ROS2 package + The scenario_selector ROS 2 package Kenji Miyake Taiki Tanaka Tomoya Kimura @@ -17,13 +17,13 @@ Kenji Miyake ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_mapping_msgs autoware_auto_planning_msgs lanelet2_extension nav_msgs + planning_test_utils rclcpp rclcpp_components route_handler @@ -35,6 +35,7 @@ ros2cli topic_tools + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp new file mode 100644 index 0000000000000..d3d6939f49b83 --- /dev/null +++ b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp @@ -0,0 +1,122 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "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 + +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: scenario_selector → test_node_ + test_manager->setScenarioSubscriber("output/scenario"); + + test_manager->setOdometryTopicName("input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.append_parameter_override("update_rate", 10.0); + node_options.append_parameter_override("th_max_message_delay_sec", INFINITY); + node_options.append_parameter_override("th_arrived_distance_m", 1.0); + node_options.append_parameter_override("th_stopped_time_sec", 1.0); + node_options.append_parameter_override("th_stopped_velocity_mps", 0.01); + auto test_target_node = std::make_shared(node_options); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishOdometry(test_target_node, "input/odometry"); + test_manager->publishParkingState(test_target_node, "is_parking_completed"); + test_manager->publishTrajectory(test_target_node, "input/parking/trajectory"); + test_manager->publishMap(test_target_node, "input/lanelet_map"); + test_manager->publishRoute(test_target_node, "input/route"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryLaneDrivingMode) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // set scenario_selector's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryParkingMode) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // set scenario_selector's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // set scenario_selector's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + rclcpp::shutdown(); +} diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 0cdd6b58c74a9..c30a9f2807df3 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -11,8 +11,8 @@ Takayuki Murooka ament_cmake_auto + autoware_cmake - autoware_cmake rosidl_default_generators autoware_auto_mapping_msgs 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 f2a7b1885198c..ab2788a7c755b 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -233,7 +233,6 @@ void StaticCenterlineOptimizerNode::run() load_map(lanelet2_input_file_path); const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); const auto optimized_traj_points = plan_path(route_lane_ids); - evaluate(route_lane_ids, optimized_traj_points); save_map(lanelet2_output_file_path, route_lane_ids, optimized_traj_points); } @@ -445,6 +444,9 @@ void StaticCenterlineOptimizerNode::on_plan_path( return; } + // publish unsafe_footprints + evaluate(route_lane_ids, optimized_traj_points); + // create output data auto target_traj_point = optimized_traj_points.cbegin(); bool is_end_lanelet = false; @@ -486,9 +488,13 @@ void StaticCenterlineOptimizerNode::evaluate( const std::vector & optimized_traj_points) { const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - const auto dist_thresh_vec = declare_parameter>("marker_color_dist_thresh"); - const auto marker_color_vec = declare_parameter>("marker_color"); + const auto dist_thresh_vec = + has_parameter("marker_color_dist_thresh") + ? get_parameter("marker_color_dist_thresh").as_double_array() + : declare_parameter>("marker_color_dist_thresh"); + const auto marker_color_vec = has_parameter("marker_color") + ? get_parameter("marker_color").as_string_array() + : declare_parameter>("marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 22fcb924597cf..3fe544c9b561e 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -91,7 +91,7 @@ PathWithLaneId get_path_with_lane_id( constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, vehicle_length, planner_data); + path_with_lane_id, drivable_lanes, false, vehicle_length, planner_data); return path_with_lane_id; } diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index 89a91c6b6a675..de3e4bf120b13 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index e1d31acbaba1c..1f99ba8d2fcba 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -10,10 +10,9 @@ Satoshi Ota ament_cmake + autoware_cmake eigen3_cmake_module - autoware_cmake - autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 8f7b0a9e0d306..17a47be8c0596 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -196,7 +196,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio void SurroundObstacleCheckerNode::onTimer() { if (!odometry_ptr_) { - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current velocity..."); return; } @@ -206,13 +206,13 @@ void SurroundObstacleCheckerNode::onTimer() } if (node_param_.use_pointcloud && !pointcloud_ptr_) { - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); return; } if (node_param_.use_dynamic_object && !object_ptr_) { - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); return; } @@ -333,6 +333,9 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { + if (pointcloud_ptr_->data.empty()) { + return boost::none; + } const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); @@ -346,7 +349,8 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast(); pcl::PointCloud transformed_pointcloud; pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); - pcl::transformPointCloud(transformed_pointcloud, transformed_pointcloud, isometry); + tier4_autoware_utils::transformPointCloud( + transformed_pointcloud, transformed_pointcloud, isometry); const auto ego_polygon = createSelfPolygon(vehicle_info_); diff --git a/sensing/geo_pos_conv/CHANGELOG.rst b/sensing/geo_pos_conv/CHANGELOG.rst index ead6db443a672..12d311b5aaa5d 100644 --- a/sensing/geo_pos_conv/CHANGELOG.rst +++ b/sensing/geo_pos_conv/CHANGELOG.rst @@ -4,7 +4,7 @@ Changelog for package geo_pos_conv 2.0.0 (2020-10-03) ------------------ -* Convert package to ROS2 +* Convert package to ROS 2 1.11.0 (2019-03-21) ------------------- diff --git a/sensing/geo_pos_conv/package.xml b/sensing/geo_pos_conv/package.xml index 0d7cbe47fdec1..b82ea443ef7d5 100644 --- a/sensing/geo_pos_conv/package.xml +++ b/sensing/geo_pos_conv/package.xml @@ -3,14 +3,13 @@ geo_pos_conv 2.0.0 - The ROS2 geo_pos_conv package + The ROS 2 geo_pos_conv package Yamato Ando Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 250122b929e7f..4887494614a81 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -3,14 +3,14 @@ gnss_poser 1.0.0 - The ROS2 gnss_poser package + The ROS 2 gnss_poser package Yamato Ando Apache License 2.0 Ryo Watanabe ament_cmake_auto + autoware_cmake - autoware_cmake libboost-dev autoware_sensing_msgs diff --git a/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp b/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp index 9f4087901c0c8..d6d150220555f 100644 --- a/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp +++ b/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp @@ -26,7 +26,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/sensing/image_transport_decompressor/package.xml b/sensing/image_transport_decompressor/package.xml index 631e72d1c4294..264a8f249da3f 100644 --- a/sensing/image_transport_decompressor/package.xml +++ b/sensing/image_transport_decompressor/package.xml @@ -17,8 +17,7 @@ Julius Kammerl ament_cmake_auto - - autoware_cmake + autoware_cmake cv_bridge rclcpp diff --git a/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp b/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp index 9d0b7997e0be0..4244facf7af12 100644 --- a/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp +++ b/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp @@ -53,7 +53,11 @@ #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index ce85a42186621..bd067e00e4c4e 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -3,14 +3,13 @@ imu_corrector 1.0.0 - The ROS2 imu_corrector package + The ROS 2 imu_corrector package Yamato Ando Apache License 2.0 Yamato Ando ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp rclcpp_components diff --git a/sensing/livox/livox_tag_filter/package.xml b/sensing/livox/livox_tag_filter/package.xml index 50f80e4c5b47d..f60bef1de5f10 100644 --- a/sensing/livox/livox_tag_filter/package.xml +++ b/sensing/livox/livox_tag_filter/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake libpcl-all-dev pcl_conversions diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 1cd4a6f1245c9..42ca694235a00 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,11 +22,13 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------ | ------------- | ----------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 54fb4c2fa2307..116dd2f832b93 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -27,7 +27,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include @@ -41,8 +45,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter { protected: virtual void filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output); + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp index dbbf60c4442f9..919ddc3b0ea1e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -26,7 +26,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include @@ -49,8 +53,7 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter { protected: virtual void filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output); + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index b517b2be78b47..a906eb913d863 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -20,6 +20,7 @@ #include +#include #include namespace pointcloud_preprocessor @@ -33,10 +34,18 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform + // to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info); + private: double distance_ratio_; double object_length_threshold_; int num_points_threshold_; + uint16_t max_rings_num_; + size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -45,18 +54,18 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); bool isCluster( - sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, - const std::vector & tmp_indices) + const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) { - PointXYZI * front_pt = reinterpret_cast(&input_ptr->data[tmp_indices.front()]); - PointXYZI * back_pt = reinterpret_cast(&input_ptr->data[tmp_indices.back()]); - - const auto x_diff = front_pt->x - back_pt->x; - const auto y_diff = front_pt->y - back_pt->y; - const auto z_diff = front_pt->z - back_pt->z; - return static_cast(tmp_indices.size()) > num_points_threshold_ || - (x_diff * x_diff) + (y_diff * y_diff) + (z_diff * z_diff) >= - object_length_threshold_ * object_length_threshold_; + if (walk_size > num_points_threshold_) return true; + + auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); + auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); + + const auto x = first_point->x - last_point->x; + const auto y = first_point->y - last_point->y; + const auto z = first_point->z - last_point->z; + + return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; } public: diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index d39bbd1fad1d6..74209c1a22e4c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -34,8 +34,7 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte { private: void filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) override; + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; rclcpp::Subscription::SharedPtr map_sub_; lanelet::ConstPolygons3d polygon_lanelets_; diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 856f7d2bc6f90..4b406c55251e2 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -3,7 +3,7 @@ pointcloud_preprocessor 0.1.0 - The ROS2 pointcloud_preprocessor package + The ROS 2 pointcloud_preprocessor package amc-nu Yukihiro Saito Shunsuke Miura @@ -15,8 +15,7 @@ William Woodall ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs autoware_point_types diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 0f740eee6be40..3563bf2b6c408 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -106,10 +106,12 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) } void BlockageDiagComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } uint horizontal_bins = static_cast((angle_range_deg_[1] - angle_range_deg_[0])); uint vertical_bins = vertical_bins_; pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index bda8682a9d485..0cae2c90c2f16 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -112,12 +112,16 @@ void CropBoxFilterComponent::filter( // TODO(sykwer): Temporary Implementation: Rename this function to `filter()` when all the filter // nodes conform to new API. Then delete the old `filter()` defined above. void CropBoxFilterComponent::faster_filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output, const TransformInfo & transform_info) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } + int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset; int y_offset = input->fields[pcl::getFieldIndex(*input, "y")].offset; int z_offset = input->fields[pcl::getFieldIndex(*input, "z")].offset; @@ -127,19 +131,19 @@ void CropBoxFilterComponent::faster_filter( for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); global_offset += input->point_step) { - Eigen::Vector4f point( - *reinterpret_cast(&input->data[global_offset + x_offset]), - *reinterpret_cast(&input->data[global_offset + y_offset]), - *reinterpret_cast(&input->data[global_offset + z_offset]), 1); + Eigen::Vector4f point; + std::memcpy(&point[0], &input->data[global_offset + x_offset], sizeof(float)); + std::memcpy(&point[1], &input->data[global_offset + y_offset], sizeof(float)); + std::memcpy(&point[2], &input->data[global_offset + z_offset], sizeof(float)); + point[3] = 1; + + if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) { + RCLCPP_WARN(this->get_logger(), "Ignoring point containing NaN values"); + continue; + } if (transform_info.need_transform) { - if (std::isfinite(point[0]) && std::isfinite(point[1]) && std::isfinite(point[2])) { - point = transform_info.eigen_transform * point; - } else { - // TODO(sykwer): Implement the appropriate logic for `max range point` and `invalid point`. - // https://github.com/ros-perception/perception_pcl/blob/628aaec1dc73ef4adea01e9d28f11eb417b948fd/pcl_ros/src/transforms.cpp#L185-L201 - RCLCPP_ERROR(this->get_logger(), "Not implemented logic"); - } + point = transform_info.eigen_transform * point; } bool point_is_inside = point[2] > param_.min_z && point[2] < param_.max_z && @@ -149,9 +153,9 @@ void CropBoxFilterComponent::faster_filter( memcpy(&output.data[output_size], &input->data[global_offset], input->point_step); if (transform_info.need_transform) { - *reinterpret_cast(&output.data[output_size + x_offset]) = point[0]; - *reinterpret_cast(&output.data[output_size + y_offset]) = point[1]; - *reinterpret_cast(&output.data[output_size + z_offset]) = point[2]; + std::memcpy(&output.data[output_size + x_offset], &point[0], sizeof(float)); + std::memcpy(&output.data[output_size + y_offset], &point[1], sizeof(float)); + std::memcpy(&output.data[output_size + z_offset], &point[2], sizeof(float)); } output_size += input->point_step; diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp index 8e6747d85da8f..6a9a5818b19ce 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp @@ -75,9 +75,12 @@ ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent( } void ApproximateDownsampleFilterComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp index 3bcf0099cc78f..6216b1dad97e8 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp @@ -67,9 +67,12 @@ RandomDownsampleFilterComponent::RandomDownsampleFilterComponent( } void RandomDownsampleFilterComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index edc4d3e8316cf..5b22986a1b6b8 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -75,9 +75,12 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( } void VoxelGridDownsampleFilterComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index 8864b90023dc1..f0b716c73b5f1 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -56,6 +56,7 @@ #include #include +#include #include #include #include @@ -123,8 +124,10 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback` // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. - auto callback = filter_name == "CropBoxFilter" ? &Filter::faster_input_indices_callback - : &Filter::input_indices_callback; + std::set supported_nodes = {"CropBoxFilter", "RingOutlierFilter"}; + auto callback = supported_nodes.find(filter_name) != supported_nodes.end() + ? &Filter::faster_input_indices_callback + : &Filter::input_indices_callback; if (use_indices_) { // Subscribe to the input using a filter diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index 139cf56dccce7..e0cc9a5964a09 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -112,10 +112,12 @@ void DualReturnOutlierFilterComponent::onVisibilityChecker(DiagnosticStatusWrapp } void DualReturnOutlierFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp index 4a905d216bb3f..bbf01e0bdf223 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp @@ -40,10 +40,12 @@ RadiusSearch2DOutlierFilterComponent::RadiusSearch2DOutlierFilterComponent( } void RadiusSearch2DOutlierFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pcl::PointCloud::Ptr xyz_cloud(new pcl::PointCloud); pcl::fromROSMsg(*input, *xyz_cloud); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 65581f0c31d19..eaffaa35bdf58 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -37,6 +37,9 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions object_length_threshold_ = static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); + max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); + max_points_num_per_ring_ = + static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -44,82 +47,146 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions std::bind(&RingOutlierFilterComponent::paramCallback, this, _1)); } -void RingOutlierFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) +// TODO(sykwer): Temporary Implementation: Rename this function to `filter()` when all the filter +// nodes conform to new API. Then delete the old `filter()` defined below. +void RingOutlierFilterComponent::faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & unused_indices, PointCloud2 & output, + const TransformInfo & transform_info) { std::scoped_lock lock(mutex_); + if (unused_indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } stop_watch_ptr_->toc("processing_time", true); - std::unordered_map> input_ring_map; - input_ring_map.reserve(128); - sensor_msgs::msg::PointCloud2::SharedPtr input_ptr = - std::make_shared(*input); + + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width); + size_t output_size = 0; const auto ring_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - for (std::size_t idx = 0U; idx < input_ptr->data.size(); idx += input_ptr->point_step) { - input_ring_map[*reinterpret_cast(&input_ptr->data[idx + ring_offset])].push_back( - idx); - } - - PointCloud2Modifier output_modifier{output, input->header.frame_id}; - output_modifier.reserve(input->width); - - std::vector tmp_indices; - tmp_indices.reserve(input->width); - const auto azimuth_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; const auto distance_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - for (const auto & ring_indices : input_ring_map) { - if (ring_indices.second.size() < 2) { - continue; - } - for (size_t idx = 0U; idx < ring_indices.second.size() - 1; ++idx) { - const auto & current_idx = ring_indices.second.at(idx); - const auto & next_idx = ring_indices.second.at(idx + 1); - tmp_indices.emplace_back(current_idx); + std::vector> ring2indices; + ring2indices.reserve(max_rings_num_); + + for (uint16_t i = 0; i < max_rings_num_; i++) { + ring2indices.push_back(std::vector()); + ring2indices.back().reserve(max_points_num_per_ring_); + } + + for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { + const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); + ring2indices[ring].push_back(data_idx); + } + + // walk range: [walk_first_idx, walk_last_idx] + int walk_first_idx = 0; + int walk_last_idx = -1; + + for (const auto & indices : ring2indices) { + if (indices.size() < 2) continue; + + walk_first_idx = 0; + walk_last_idx = -1; + + for (size_t idx = 0U; idx < indices.size() - 1; ++idx) { + const size_t & current_data_idx = indices[idx]; + const size_t & next_data_idx = indices[idx + 1]; + walk_last_idx = idx; // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) - const auto current_pt_azimuth = - *reinterpret_cast(&input_ptr->data[current_idx + azimuth_offset]); - const auto next_pt_azimuth = - *reinterpret_cast(&input_ptr->data[next_idx + azimuth_offset]); - float azimuth_diff = next_pt_azimuth - current_pt_azimuth; + + const float & current_azimuth = + *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); + const float & next_azimuth = + *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); + float azimuth_diff = next_azimuth - current_azimuth; azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; - const auto current_pt_distance = - *reinterpret_cast(&input_ptr->data[current_idx + distance_offset]); - const auto next_pt_distance = - *reinterpret_cast(&input_ptr->data[next_idx + distance_offset]); + const float & current_distance = + *reinterpret_cast(&input->data[current_data_idx + distance_offset]); + const float & next_distance = + *reinterpret_cast(&input->data[next_data_idx + distance_offset]); if ( - std::max(current_pt_distance, next_pt_distance) < - std::min(current_pt_distance, next_pt_distance) * distance_ratio_ && + std::max(current_distance, next_distance) < + std::min(current_distance, next_distance) * distance_ratio_ && azimuth_diff < 100.f) { - continue; + continue; // Determined to be included in the same walk } - if (isCluster(input_ptr, tmp_indices)) { - for (const auto & tmp_idx : tmp_indices) { - output_modifier.push_back( - std::move(*reinterpret_cast(&input_ptr->data[tmp_idx]))); + + if (isCluster( + input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), + walk_last_idx - walk_first_idx + 1)) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + output_ptr->x = p[0]; + output_ptr->y = p[1]; + output_ptr->z = p[2]; + output_ptr->intensity = input_ptr->intensity; + } else { + *output_ptr = *input_ptr; + } + + output_size += output.point_step; } } - tmp_indices.clear(); - } - if (tmp_indices.empty()) { - continue; + + walk_first_idx = idx + 1; } - if (isCluster(input_ptr, tmp_indices)) { - for (const auto & tmp_idx : tmp_indices) { - output_modifier.push_back( - std::move(*reinterpret_cast(&input_ptr->data[tmp_idx]))); + + if (walk_first_idx > walk_last_idx) continue; + + if (isCluster( + input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), + walk_last_idx - walk_first_idx + 1)) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + output_ptr->x = p[0]; + output_ptr->y = p[1]; + output_ptr->z = p[2]; + output_ptr->intensity = input_ptr->intensity; + } else { + *output_ptr = *input_ptr; + } + + output_size += output.point_step; } } - tmp_indices.clear(); } + + output.data.resize(output_size); + + // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform + // == true` + output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_; + + output.fields.resize(4); // x, y, z, intensity + std::copy( + input->fields.begin(), + input->fields.begin() + static_cast(autoware_point_types::PointIndex::Intensity) + 1, + output.fields.begin()); + + output.height = 1; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; + output.width = static_cast(output.data.size() / output.height / output.point_step); + output.row_step = static_cast(output.data.size() / output.height); + // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); @@ -131,6 +198,17 @@ void RingOutlierFilterComponent::filter( } } +// TODO(sykwer): Temporary Implementation: Delete this function definition when all the filter nodes +// conform to new API +void RingOutlierFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallback( const std::vector & p) { diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp index c65533011987b..8745c20a7cb0b 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp @@ -39,10 +39,12 @@ VoxelGridOutlierFilterComponent::VoxelGridOutlierFilterComponent( std::bind(&VoxelGridOutlierFilterComponent::paramCallback, this, _1)); } void VoxelGridOutlierFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_voxelized_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp index 35ef8f61da579..6011c92afaeb0 100644 --- a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp @@ -67,10 +67,12 @@ PassThroughFilterComponent::PassThroughFilterComponent(const rclcpp::NodeOptions } void PassThroughFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } output = *input; } diff --git a/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp b/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp index fdb6bcc9102cb..cfaffece4169f 100644 --- a/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp @@ -34,10 +34,12 @@ PointcloudAccumulatorComponent::PointcloudAccumulatorComponent(const rclcpp::Nod } void PointcloudAccumulatorComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } pointcloud_buffer_.push_front(input); rclcpp::Time last_time = input->header.stamp; pcl::PointCloud pcl_input; diff --git a/sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp b/sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp index edab53e3145c9..6cdfc8bbe2c05 100644 --- a/sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp +++ b/sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp @@ -49,9 +49,12 @@ PolygonRemoverComponent::PolygonRemoverComponent(const rclcpp::NodeOptions & opt } void PolygonRemoverComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } + if (!this->polygon_is_initialized_) { RCLCPP_INFO_STREAM(get_logger(), "Polygon is not initialized, publishing incoming cloud."); output = *input; diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index a550b196e1d21..5b8755714b375 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -77,9 +77,12 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( } void VectorMapInsideAreaFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } + if (polygon_lanelets_.empty()) { output = *input; return; diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index dc6ba43812ce6..2360e8b33901d 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake pcl_conversions pcl_ros @@ -17,7 +18,6 @@ rclcpp_components sensor_msgs - autoware_cmake ament_clang_format ament_lint_auto ament_lint_common diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index 30739d5578cb9..a15aa43d71cf6 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake message_filters nav_msgs @@ -20,7 +21,6 @@ tf2_ros tier4_autoware_utils - autoware_cmake ament_clang_format ament_lint_auto ament_lint_common diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/radar_threshold_filter/package.xml index ab364d2b3a834..00bb530567dc4 100644 --- a/sensing/radar_threshold_filter/package.xml +++ b/sensing/radar_threshold_filter/package.xml @@ -9,12 +9,12 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake radar_msgs rclcpp rclcpp_components - autoware_cmake ament_clang_format ament_lint_auto ament_lint_common diff --git a/sensing/tier4_pcl_extensions/package.xml b/sensing/tier4_pcl_extensions/package.xml index 0e29da999a449..22b053bd7c788 100644 --- a/sensing/tier4_pcl_extensions/package.xml +++ b/sensing/tier4_pcl_extensions/package.xml @@ -8,11 +8,11 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake eigen3_cmake_module eigen3_cmake_module - autoware_cmake eigen range-v3 diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index e6816afb8b511..c44c55bcd40eb 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs geometry_msgs diff --git a/simulator/dummy_perception_publisher/README.md b/simulator/dummy_perception_publisher/README.md index db2796433fba1..cfd5eb341ad5b 100644 --- a/simulator/dummy_perception_publisher/README.md +++ b/simulator/dummy_perception_publisher/README.md @@ -24,12 +24,13 @@ This node publishes the result of the dummy detection with the type of perceptio ## Parameters -| Name | Type | Default Value | Explanation | -| --------------------------- | ------ | ------------- | ------------------------------------------- | -| `visible_range` | double | 100.0 | sensor visible range [m] | -| `detection_successful_rate` | double | 0.8 | sensor detection rate. (min) 0.0 - 1.0(max) | -| `enable_ray_tracing` | bool | true | if True, use ray tracking | -| `use_object_recognition` | bool | true | if True, publish objects topic | +| Name | Type | Default Value | Explanation | +| --------------------------- | ------ | ------------- | ------------------------------------------------ | +| `visible_range` | double | 100.0 | sensor visible range [m] | +| `detection_successful_rate` | double | 0.8 | sensor detection rate. (min) 0.0 - 1.0(max) | +| `enable_ray_tracing` | bool | true | if True, use ray tracking | +| `use_object_recognition` | bool | true | if True, publish objects topic | +| `use_base_link_z` | bool | true | if True, node uses z coordinate of ego base_link | ### Node Parameters diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index ea314207dc203..0c859838bbe42 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -116,6 +116,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node bool enable_ray_tracing_; bool use_object_recognition_; bool use_real_param_; + bool use_base_link_z_; std::unique_ptr pointcloud_creator_; double angle_increment_; diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 0d17feffed21f..3184bcb234957 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -4,6 +4,7 @@ + @@ -19,6 +20,7 @@ + @@ -36,6 +38,7 @@ + diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index b24a015d21ddd..0632bd90b2c22 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -8,8 +8,8 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake rosidl_default_generators rosidl_default_runtime diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 13dc59b2a9add..41b336ba84a0b 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -106,7 +106,7 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() detection_successful_rate_ = this->declare_parameter("detection_successful_rate", 0.8); enable_ray_tracing_ = this->declare_parameter("enable_ray_tracing", true); use_object_recognition_ = this->declare_parameter("use_object_recognition", true); - + use_base_link_z_ = this->declare_parameter("use_base_link_z", true); const bool object_centric_pointcloud = this->declare_parameter("object_centric_pointcloud", false); @@ -295,15 +295,17 @@ void DummyPerceptionPublisherNode::objectCallback( tf2::toMsg(tf_map2object_origin, object.initial_state.pose_covariance.pose); // Use base_link Z - geometry_msgs::msg::TransformStamped ros_map2base_link; - try { - ros_map2base_link = tf_buffer_.lookupTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); - object.initial_state.pose_covariance.pose.position.z = - ros_map2base_link.transform.translation.z; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - return; + if (use_base_link_z_) { + geometry_msgs::msg::TransformStamped ros_map2base_link; + try { + ros_map2base_link = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); + object.initial_state.pose_covariance.pose.position.z = + ros_map2base_link.transform.translation.z; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + return; + } } objects_.push_back(object); @@ -339,18 +341,20 @@ void DummyPerceptionPublisherNode::objectCallback( dummy_perception_publisher::msg::Object object; objects_.at(i) = *msg; tf2::toMsg(tf_map2object_origin, objects_.at(i).initial_state.pose_covariance.pose); - - // Use base_link Z - geometry_msgs::msg::TransformStamped ros_map2base_link; - try { - ros_map2base_link = tf_buffer_.lookupTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); - objects_.at(i).initial_state.pose_covariance.pose.position.z = - ros_map2base_link.transform.translation.z; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - return; + if (use_base_link_z_) { + // Use base_link Z + geometry_msgs::msg::TransformStamped ros_map2base_link; + try { + ros_map2base_link = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); + objects_.at(i).initial_state.pose_covariance.pose.position.z = + ros_map2base_link.transform.translation.z; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + return; + } } + break; } } diff --git a/simulator/fault_injection/package.xml b/simulator/fault_injection/package.xml index 3b3cd5ca6c45b..e169a338b4e52 100644 --- a/simulator/fault_injection/package.xml +++ b/simulator/fault_injection/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_aggregator diagnostic_msgs diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 057b2418450e1..9e7c101e76407 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -15,10 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include + #include #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index bc6e807144b52..1beec46a48ee9 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -15,10 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include + #include #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 047742a2e8fa2..30cf5d3c7c24a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -15,10 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include + #include #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp index d48e744ee93ea..e4fcaa59f1294 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -15,10 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include + #include /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index 95e105592d2ea..55b5ddb8fcec5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -15,10 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include + #include /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp index cc4b1b4ea3651..09046cab89dc4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -15,10 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include + #include /** * @class SimModelIdealSteerVel diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 40711cfe9e720..981fb0f416d72 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -15,7 +15,7 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include "eigen3/Eigen/Core" +#include #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index ca4766f875dbe..c83433d35bb1b 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -9,8 +9,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_planning_msgs diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index 12f3e8088d7d3..3f489d9ba29a2 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -8,8 +8,8 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake libboost-serialization-dev diagnostic_msgs diff --git a/system/component_state_monitor/package.xml b/system/component_state_monitor/package.xml index 310de4fcc0016..ce20787704035 100644 --- a/system/component_state_monitor/package.xml +++ b/system/component_state_monitor/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_msgs rclcpp diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index f6f43a82f7448..d99037fdc42d0 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -12,8 +12,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_ad_api_specs autoware_adapi_v1_msgs diff --git a/system/default_ad_api/src/motion.cpp b/system/default_ad_api/src/motion.cpp index b02fd43e9be44..9d4461fc76f1e 100644 --- a/system/default_ad_api/src/motion.cpp +++ b/system/default_ad_api/src/motion.cpp @@ -105,6 +105,17 @@ void MotionNode::change_state(const State state) pub_state_->publish(msg); } state_ = state; + update_pause(state); +} + +void MotionNode::update_pause(const State state) +{ + if (state == State::Pausing) { + return change_pause(true); + } + if (state == State::Resuming) { + return change_pause(false); + } } void MotionNode::change_pause(bool pause) @@ -120,24 +131,19 @@ void MotionNode::change_pause(bool pause) void MotionNode::on_timer() { update_state(); - - if (state_ == State::Pausing) { - return change_pause(true); - } - if (state_ == State::Resuming) { - return change_pause(false); - } } void MotionNode::on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg) { is_paused_ = msg->data; + update_state(); } void MotionNode::on_is_start_requested( const control_interface::IsStartRequested::Message::ConstSharedPtr msg) { is_start_requested_ = msg->data; + update_state(); } void MotionNode::on_accept( diff --git a/system/default_ad_api/src/motion.hpp b/system/default_ad_api/src/motion.hpp index 4d929d80c9655..00c911da6bd2d 100644 --- a/system/default_ad_api/src/motion.hpp +++ b/system/default_ad_api/src/motion.hpp @@ -54,6 +54,7 @@ class MotionNode : public rclcpp::Node void update_state(); void change_state(const State state); + void update_pause(const State state); void change_pause(bool pause); void on_timer(); void on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg); diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 57daa8f840428..d464197e03603 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -85,7 +85,7 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning std::vector steering_factor_topics = { "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", "/planning/steering_factor/lane_change", "/planning/steering_factor/pull_out", - "/planning/steering_factor/pull_over"}; + "/planning/steering_factor/goal_planner"}; sub_velocity_factors_ = init_factors(this, velocity_factors_, velocity_factor_topics); diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index f2181eea5fb74..5204146201c8a 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -33,6 +33,8 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); adaptor.relay_service(cli_set_route_, srv_set_route_, group_cli_); adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); + adaptor.relay_service(cli_change_route_, srv_change_route_, group_cli_); + adaptor.relay_service(cli_change_route_points_, srv_change_route_points_, group_cli_); adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index a0b1626d42807..7c15f6d64608f 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -38,11 +38,15 @@ class RoutingNode : public rclcpp::Node Pub pub_route_; Srv srv_set_route_points_; Srv srv_set_route_; + Srv srv_change_route_points_; + Srv srv_change_route_; Srv srv_clear_route_; Sub sub_state_; Sub sub_route_; Cli cli_set_route_points_; Cli cli_set_route_; + Cli cli_change_route_points_; + Cli cli_change_route_; Cli cli_clear_route_; Cli cli_operation_mode_; Sub sub_operation_mode_; diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index b0dd92df3a95b..6cb43c5475828 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -24,6 +24,7 @@ The clear API is called automatically before setting the route. | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | +| Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. | | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml index 02f873813520c..4ed9d134d2183 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml @@ -11,6 +11,7 @@ + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index 7774c8acbe945..e54a5faa6e1b7 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -12,8 +12,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_ad_api_specs autoware_adapi_v1_msgs diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp index 4823955474d06..7151902a972d6 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp @@ -27,10 +27,13 @@ RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor") "~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1)); sub_rough_goal_ = create_subscription( "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1)); + sub_reroute_ = create_subscription( + "~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1)); sub_waypoint_ = create_subscription( "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1)); const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_cli(cli_reroute_); adaptor.init_cli(cli_route_); adaptor.init_cli(cli_clear_); adaptor.init_sub( @@ -97,6 +100,14 @@ void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose) route_->waypoints.push_back(pose->pose); } +void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose) +{ + const auto route = std::make_shared(); + route->header = pose->header; + route->goal = pose->pose; + cli_reroute_->async_send_request(route); +} + } // namespace ad_api_adaptors int main(int argc, char ** argv) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp index 6c0bbf72e40a7..4040ee37ead3e 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp @@ -34,14 +34,17 @@ class RoutingAdaptor : public rclcpp::Node private: using PoseStamped = geometry_msgs::msg::PoseStamped; using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints; + using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints; using ClearRoute = autoware_ad_api::routing::ClearRoute; using RouteState = autoware_ad_api::routing::RouteState; + component_interface_utils::Client::SharedPtr cli_reroute_; component_interface_utils::Client::SharedPtr cli_route_; component_interface_utils::Client::SharedPtr cli_clear_; component_interface_utils::Subscription::SharedPtr sub_state_; rclcpp::Subscription::SharedPtr sub_fixed_goal_; rclcpp::Subscription::SharedPtr sub_rough_goal_; rclcpp::Subscription::SharedPtr sub_waypoint_; + rclcpp::Subscription::SharedPtr sub_reroute_; rclcpp::TimerBase::SharedPtr timer_; bool calling_service_ = false; @@ -53,6 +56,7 @@ class RoutingAdaptor : public rclcpp::Node void on_fixed_goal(const PoseStamped::ConstSharedPtr pose); void on_rough_goal(const PoseStamped::ConstSharedPtr pose); void on_waypoint(const PoseStamped::ConstSharedPtr pose); + void on_reroute(const PoseStamped::ConstSharedPtr pose); }; } // namespace ad_api_adaptors diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py index 194551d56dcd5..7bfa72101ade4 100755 --- a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py +++ b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py @@ -40,7 +40,7 @@ SteeringFactor.AVOIDANCE_PATH_RETURN: "avoidance return", SteeringFactor.STATION: "station", SteeringFactor.PULL_OUT: "pull out", - SteeringFactor.PULL_OVER: "pull over", + SteeringFactor.GOAL_PLANNER: "goal planner", SteeringFactor.EMERGENCY_OPERATION: "emergency operation", } diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml index a8172d1ced9ab..10f4abfe446fb 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -12,8 +12,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_ad_api_specs autoware_adapi_v1_msgs diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index 191facf6e3777..86da760b99a46 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -3,15 +3,14 @@ dummy_diag_publisher 0.1.0 - The dummy_diag_publisher ROS2 package + The dummy_diag_publisher ROS 2 package Kenji Miyake Akihiro Sakurai Fumihito Ito Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_updater fmt diff --git a/system/dummy_infrastructure/package.xml b/system/dummy_infrastructure/package.xml index a361656545306..654feb5ffc7cf 100644 --- a/system/dummy_infrastructure/package.xml +++ b/system/dummy_infrastructure/package.xml @@ -8,8 +8,8 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake - autoware_cmake libboost-dev rclcpp diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 987e44ad340d5..6d83c6781fad7 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -29,7 +29,7 @@ #include #include -// ROS2 core +// ROS 2 core #include #include diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index babcd28537459..4ee6674b2f569 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -3,14 +3,13 @@ emergency_handler 0.1.0 - The emergency_handler ROS2 package + The emergency_handler ROS 2 package Kenji Miyake Makoto Kurihara Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_control_msgs diff --git a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp index c6f9a4be8902a..7856c23fa3158 100644 --- a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp +++ b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp @@ -25,7 +25,7 @@ #include #include -// ROS2 core +// ROS 2 core #include namespace mrm_comfortable_stop_operator diff --git a/system/mrm_comfortable_stop_operator/package.xml b/system/mrm_comfortable_stop_operator/package.xml index a108f52f4bd66..881d1f0e81bd7 100644 --- a/system/mrm_comfortable_stop_operator/package.xml +++ b/system/mrm_comfortable_stop_operator/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs rclcpp diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index 2ac6321caab55..19e41397fe4da 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -24,7 +24,7 @@ #include #include -// ROS2 core +// ROS 2 core #include namespace mrm_emergency_stop_operator diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 0761a1cbb9980..54ad68f00537a 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_adapi_v1_msgs autoware_auto_control_msgs diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index be25f799af457..ba8a682204f5f 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -3,14 +3,13 @@ system_error_monitor 0.1.1 - The system_error_monitor package in ROS2 + The system_error_monitor package in ROS 2 Kenji Miyake Fumihito Ito Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_system_msgs autoware_auto_vehicle_msgs diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 57f67a247d2b9..1bd7caa19a220 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -213,7 +213,11 @@ int isInNoFaultCondition( AutowareErrorMonitor::AutowareErrorMonitor() : Node( "system_error_monitor", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), + diag_array_stamp_(0, 0, this->get_clock()->get_clock_type()), + autoware_state_stamp_(0, 0, this->get_clock()->get_clock_type()), + current_gate_mode_stamp_(0, 0, this->get_clock()->get_clock_type()), + control_mode_stamp_(0, 0, this->get_clock()->get_clock_type()) { // Parameter get_parameter_or("update_rate", params_.update_rate, 10); @@ -385,7 +389,7 @@ void AutowareErrorMonitor::onControlMode( bool AutowareErrorMonitor::isDataReady() { if (!diag_array_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for diag_array msg..."); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for diag_array msg..."); return false; } diff --git a/system/system_monitor/CMakeLists.txt b/system/system_monitor/CMakeLists.txt index 391c82a09c960..be3f8fadc8a81 100644 --- a/system/system_monitor/CMakeLists.txt +++ b/system/system_monitor/CMakeLists.txt @@ -186,7 +186,7 @@ rclcpp_components_register_node(voltage_monitor_lib EXECUTABLE voltage_monitor ) -# TODO(yunus.caliskan): Port the tests to ROS2, robustify the tests. +# TODO(yunus.caliskan): Port the tests to ROS 2, robustify the tests. if(BUILD_TESTING) # ament_add_ros_isolated_gtest(test_cpu_monitor # test/src/cpu_monitor/test_${CMAKE_CPU_PLATFORM}_cpu_monitor.cpp diff --git a/system/system_monitor/package.xml b/system/system_monitor/package.xml index bc4b040051d3f..470e82e3dd757 100644 --- a/system/system_monitor/package.xml +++ b/system/system_monitor/package.xml @@ -9,8 +9,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake diagnostic_msgs diagnostic_updater diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 9d8b54de0773e..6053c183eda4d 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -385,11 +385,11 @@ int get_nvme_smart_data(int fd, HddInfo * info) // is from 1 to 1,000, three indicates that the number of 512 byte data // units written is from 2,001 to 3,000) info->is_valid_total_data_written_ = true; - info->total_data_written_ = *(reinterpret_cast(&data[48])); + std::memcpy(&info->total_data_written_, &data[48], sizeof(info->total_data_written_)); // Bytes 143:128 Power On Hours info->is_valid_power_on_hours_ = true; - info->power_on_hours_ = *(reinterpret_cast(&data[128])); + std::memcpy(&info->power_on_hours_, &data[128], sizeof(info->power_on_hours_)); // NVMe S.M.A.R.T has no information of recovered error count info->is_valid_recovered_error_ = false; diff --git a/system/topic_state_monitor/package.xml b/system/topic_state_monitor/package.xml index 2707a74ff5413..3405a63b4af39 100644 --- a/system/topic_state_monitor/package.xml +++ b/system/topic_state_monitor/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake ament_index_cpp diagnostic_updater diff --git a/system/velodyne_monitor/package.xml b/system/velodyne_monitor/package.xml index 9c6683511c2ce..384456673d320 100644 --- a/system/velodyne_monitor/package.xml +++ b/system/velodyne_monitor/package.xml @@ -8,8 +8,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake crypto++ diagnostic_msgs diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md index 60d652c6140e4..c8813280c04ff 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md @@ -185,7 +185,7 @@ ros2 run accel_brake_map_calibrator actuation_cmd_publisher.py ## Calibration Method -Two algorithms are selectable for the acceleration map update, [update_offset_four_cell_around](#update_offset_four_cell_around-1) and [update_offset_each_cell](#update_offset_each_cell). Please see the link for datails. +Two algorithms are selectable for the acceleration map update, [update_offset_four_cell_around](#update_offset_four_cell_around-1) and [update_offset_each_cell](#update_offset_each_cell). Please see the link for details. ### Data Preprocessing diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml index 8163a1209883c..64e7d1fb105cf 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml @@ -11,8 +11,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs diagnostic_updater diff --git a/vehicle/external_cmd_converter/package.xml b/vehicle/external_cmd_converter/package.xml index af7200c6f51bd..d9301e23e05fe 100644 --- a/vehicle/external_cmd_converter/package.xml +++ b/vehicle/external_cmd_converter/package.xml @@ -10,8 +10,7 @@ Takamasa Horibe ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_vehicle_msgs diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index c48f2c0bed773..d68f4601aab67 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -15,8 +15,7 @@ Tanaka Taiki ament_cmake_auto - - autoware_cmake + autoware_cmake autoware_auto_control_msgs autoware_auto_vehicle_msgs diff --git a/vehicle/steer_offset_estimator/package.xml b/vehicle/steer_offset_estimator/package.xml index f2c01c9fb2fc4..242ec5f2f148a 100644 --- a/vehicle/steer_offset_estimator/package.xml +++ b/vehicle/steer_offset_estimator/package.xml @@ -7,7 +7,7 @@ Apache License 2.0 ament_cmake_auto - autoware_cmake + autoware_cmake autoware_auto_vehicle_msgs diagnostic_updater diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/vehicle_info_util/package.xml index 4db17aa6f13be..41cb10003a1de 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/vehicle_info_util/package.xml @@ -14,8 +14,7 @@ Apache License 2.0 ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp tier4_autoware_utils