diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index a0e7bcce350f5..5e1035de20c64 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -3,7 +3,6 @@ checkersReport missingInclude missingIncludeSystem -unknownMacro unmatchedSuppression unusedFunction useInitializationList diff --git a/.cspell.json b/.cspell.json index b0d572db8726b..2d024a5ca589d 100644 --- a/.cspell.json +++ b/.cspell.json @@ -4,5 +4,5 @@ "planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen", "fromarray"] + "words": ["dltype", "tvmgen", "fromarray", "soblin"] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3367eb9ad71b5..205f6c7fb4438 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -10,6 +10,7 @@ common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodr common/autoware_path_distance_calculator/** isamu.takagi@tier4.jp common/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp +common/autoware_signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -27,7 +28,6 @@ common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp @@ -91,8 +91,8 @@ localization/autoware_pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tie localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -106,7 +106,7 @@ perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp +perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp yoshi.ri@tier4.jp kotaro.uetake@tier4.jp perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml index 3decc3f9861b1..89893e9f55fb5 100644 --- a/.github/actions/build-and-test-differential/action.yaml +++ b/.github/actions/build-and-test-differential/action.yaml @@ -57,8 +57,10 @@ runs: restore-keys: | ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - name: Show ccache stats before build - run: du -sh ${CCACHE_DIR} && ccache -s + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats shell: bash - name: Export CUDA state as a variable for adding to cache key diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index df49e5d418bc9..0f239cd328591 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -69,8 +69,10 @@ jobs: echo -e "# Set maximum cache size\nmax_size = 600MB" >> "${CCACHE_DIR}/ccache.conf" shell: bash - - name: Show ccache stats before build - run: du -sh ${CCACHE_DIR} && ccache -s + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats shell: bash - name: Export CUDA state as a variable for adding to cache key diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 6d466f7c6bc45..8c2edd3c76724 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -71,7 +71,7 @@ jobs: id: cppcheck run: | echo "Running Cppcheck on modified packages: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }}" - cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt + cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt shell: bash - name: Setup Problem Matchers for cppcheck diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml index c59d663048b9c..573dcc54aa8c0 100644 --- a/.github/workflows/cppcheck-weekly.yaml +++ b/.github/workflows/cppcheck-weekly.yaml @@ -23,7 +23,7 @@ jobs: continue-on-error: true id: cppcheck run: | - cppcheck --enable=all --inconclusive --check-level=exhaustive --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml + cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml shell: bash - name: Count errors by error ID and severity diff --git a/codecov.yaml b/codecov.yaml index 255312a29ccf7..75fc6f8bacadd 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -30,3 +30,86 @@ flag_management: ignore: - "**/test/*" - "**/test/**/*" + - "**/debug.*" + +component_management: + individual_components: + - component_id: planning-tier-iv-maintained-packages + name: Planning TIER IV Maintained Packages + paths: + - planning/autoware_costmap_generator/** + - planning/autoware_external_velocity_limit_selector/** + - planning/autoware_freespace_planner/** + - planning/autoware_freespace_planning_algorithms/** + - planning/autoware_mission_planner/** + # - planning/autoware_objects_of_interest_marker_interface/** + - planning/autoware_obstacle_cruise_planner/** + # - planning/autoware_obstacle_stop_planner/** + - planning/autoware_path_optimizer/** + - planning/autoware_path_smoother/** + - planning/autoware_planning_test_manager/** + - planning/autoware_planning_topic_converter/** + - planning/autoware_planning_validator/** + - planning/autoware_remaining_distance_time_calculator/** + - planning/autoware_route_handler/** + - planning/autoware_rtc_interface/** + - planning/autoware_scenario_selector/** + - planning/autoware_static_centerline_generator/** + - planning/autoware_surround_obstacle_checker/** + - planning/autoware_velocity_smoother/** + ##### behavior_path_planner ##### + # - planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** + - planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_planner_common/** + - planning/behavior_path_planner/autoware_behavior_path_start_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** + - planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** + # - planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/** + # - planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** + - planning/behavior_path_planner/autoware_behavior_path_planner/** + - planning/behavior_path_planner/autoware_behavior_path_side_shift_module/** + ##### behavior_velocity_planner ##### + - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** + ##### motion_velocity_planner ##### + - planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** + + - component_id: control-tier-iv-maintained-packages + name: Control TIER IV Maintained Packages + paths: + - control/autoware_autonomous_emergency_braking/** + - control/autoware_control_validator/** + - control/autoware_external_cmd_selector/** + # - control/autoware_joy_controller/** + - control/autoware_lane_departure_checker/** + - control/autoware_mpc_lateral_controller/** + - control/autoware_operation_mode_transition_manager/** + - control/autoware_pid_longitudinal_controller/** + # - control/autoware_pure_pursuit/** + - control/autoware_shift_decider/** + # - control/autoware_smart_mpc_trajectory_follower/** + - control/autoware_trajectory_follower_base/** + - control/autoware_trajectory_follower_node/** + - control/autoware_vehicle_cmd_gate/** + # - control/control_performance_analysis/** + - control/obstacle_collision_checker/** + # - control/predicted_path_checker/** diff --git a/common/interpolation/CMakeLists.txt b/common/autoware_interpolation/CMakeLists.txt similarity index 80% rename from common/interpolation/CMakeLists.txt rename to common/autoware_interpolation/CMakeLists.txt index cc91bed012432..09797272a2ac8 100644 --- a/common/interpolation/CMakeLists.txt +++ b/common/autoware_interpolation/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(interpolation) +project(autoware_interpolation) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(interpolation SHARED +ament_auto_add_library(autoware_interpolation SHARED src/linear_interpolation.cpp src/spline_interpolation.cpp src/spline_interpolation_points_2d.cpp @@ -17,7 +17,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_interpolation ${test_files}) target_link_libraries(test_interpolation - interpolation + autoware_interpolation ) endif() diff --git a/common/interpolation/README.md b/common/autoware_interpolation/README.md similarity index 100% rename from common/interpolation/README.md rename to common/autoware_interpolation/README.md diff --git a/common/interpolation/include/interpolation/interpolation_utils.hpp b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp similarity index 93% rename from common/interpolation/include/interpolation/interpolation_utils.hpp rename to common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp index 9c0372f788ecb..1c0913c8847f4 100644 --- a/common/interpolation/include/interpolation/interpolation_utils.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_ -#define INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ #include #include #include #include -namespace interpolation_utils +namespace autoware::interpolation { inline bool isIncreasing(const std::vector & x) { @@ -109,6 +109,6 @@ void validateKeysAndValues( throw std::invalid_argument("The size of base_keys and base_values are not the same."); } } -} // namespace interpolation_utils +} // namespace autoware::interpolation -#endif // INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#endif // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ diff --git a/common/interpolation/include/interpolation/linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp similarity index 75% rename from common/interpolation/include/interpolation/linear_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp index 1f8155613111e..762806b3a5c35 100644 --- a/common/interpolation/include/interpolation/linear_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_ -#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include -namespace interpolation +namespace autoware::interpolation { double lerp(const double src_val, const double dst_val, const double ratio); @@ -30,7 +30,6 @@ std::vector lerp( double lerp( const std::vector & base_keys, const std::vector & base_values, const double query_key); +} // namespace autoware::interpolation -} // namespace interpolation - -#endif // INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp similarity index 79% rename from common/interpolation/include/interpolation/spherical_linear_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp index 8c1d49fb5f607..2e4a8fbd42907 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ -#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include @@ -29,7 +29,7 @@ #include -namespace interpolation +namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, @@ -43,6 +43,6 @@ std::vector slerp( geometry_msgs::msg::Quaternion lerpOrientation( const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, const double ratio); -} // namespace interpolation +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp similarity index 81% rename from common/interpolation/include/interpolation/spline_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index 38e7b3fed5b8b..f7feada78ff4f 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_ -#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#include "autoware/interpolation/interpolation_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/interpolation_utils.hpp" + +#include #include #include @@ -24,27 +26,8 @@ #include #include -namespace interpolation -{ -// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) -struct MultiSplineCoef +namespace autoware::interpolation { - MultiSplineCoef() = default; - - explicit MultiSplineCoef(const size_t num_spline) - { - a.resize(num_spline); - b.resize(num_spline); - c.resize(num_spline); - d.resize(num_spline); - } - - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; - // static spline interpolation functions std::vector spline( const std::vector & base_keys, const std::vector & base_values, @@ -52,7 +35,6 @@ std::vector spline( std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); -} // namespace interpolation // non-static 1-dimensional spline interpolation // @@ -98,11 +80,18 @@ class SplineInterpolation size_t getSize() const { return base_keys_.size(); } private: + Eigen::VectorXd a_; + Eigen::VectorXd b_; + Eigen::VectorXd c_; + Eigen::VectorXd d_; + std::vector base_keys_; - interpolation::MultiSplineCoef multi_spline_coef_; void calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values); + + Eigen::Index get_index(const double & key) const; }; +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp similarity index 89% rename from common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp index b2170cf83bde2..c9668a4cad7eb 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp @@ -12,19 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ -#define INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include -namespace interpolation +namespace autoware::interpolation { template std::vector splineYawFromPoints(const std::vector & points); -} // namespace interpolation // non-static points spline interpolation // NOTE: We can calculate yaw from the x and y by interpolation derivatives. @@ -85,5 +84,6 @@ class SplineInterpolationPoints2d std::vector base_s_vec_; }; +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp similarity index 85% rename from common/interpolation/include/interpolation/zero_order_hold.hpp rename to common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp index 966128321b470..c28f870837da7 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__ZERO_ORDER_HOLD_HPP_ -#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#ifndef AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include -namespace interpolation +namespace autoware::interpolation { inline std::vector calc_closest_segment_indices( const std::vector & base_keys, const std::vector & query_keys, const double overlap_threshold = 1e-3) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); + const auto validated_query_keys = validateKeys(base_keys, query_keys); std::vector closest_segment_indices(validated_query_keys.size()); size_t closest_segment_idx = 0; @@ -58,7 +58,7 @@ std::vector zero_order_hold( const std::vector & closest_segment_indices) { // throw exception for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); + validateKeysAndValues(base_keys, base_values); std::vector query_values(closest_segment_indices.size()); for (size_t i = 0; i < closest_segment_indices.size(); ++i) { @@ -76,6 +76,6 @@ std::vector zero_order_hold( return zero_order_hold( base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold)); } -} // namespace interpolation +} // namespace autoware::interpolation -#endif // INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#endif // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ diff --git a/common/interpolation/package.xml b/common/autoware_interpolation/package.xml similarity index 92% rename from common/interpolation/package.xml rename to common/autoware_interpolation/package.xml index bb4af924dd252..d2538bd602f45 100644 --- a/common/interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -1,7 +1,7 @@ - interpolation + autoware_interpolation 0.1.0 The spline interpolation package Fumiya Watanabe @@ -12,6 +12,7 @@ autoware_cmake autoware_universe_utils + eigen ament_cmake_ros ament_lint_auto diff --git a/common/interpolation/src/linear_interpolation.cpp b/common/autoware_interpolation/src/linear_interpolation.cpp similarity index 86% rename from common/interpolation/src/linear_interpolation.cpp rename to common/autoware_interpolation/src/linear_interpolation.cpp index f74d085dfee9e..4ba339f0e538e 100644 --- a/common/interpolation/src/linear_interpolation.cpp +++ b/common/autoware_interpolation/src/linear_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include -namespace interpolation +namespace autoware::interpolation { double lerp(const double src_val, const double dst_val, const double ratio) { @@ -28,8 +28,8 @@ std::vector lerp( const std::vector & query_keys) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; @@ -56,4 +56,4 @@ double lerp( { return lerp(base_keys, base_values, std::vector{query_key}).front(); } -} // namespace interpolation +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp similarity index 88% rename from common/interpolation/src/spherical_linear_interpolation.cpp rename to common/autoware_interpolation/src/spherical_linear_interpolation.cpp index e44626498a80b..697dda3b06770 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" -namespace interpolation +namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, @@ -34,8 +34,8 @@ std::vector slerp( const std::vector & query_keys) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; @@ -68,4 +68,4 @@ geometry_msgs::msg::Quaternion lerpOrientation( const auto q_interpolated = q_from.slerp(q_to, ratio); return tf2::toMsg(q_interpolated); } -} // namespace interpolation +} // namespace autoware::interpolation diff --git a/common/autoware_interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp new file mode 100644 index 0000000000000..78f3778ae8e21 --- /dev/null +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -0,0 +1,247 @@ +// 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 "autoware/interpolation/spline_interpolation.hpp" + +#include +#include + +namespace autoware::interpolation +{ +Eigen::VectorXd solve_tridiagonal_matrix_algorithm( + const Eigen::Ref & a, const Eigen::Ref & b, + const Eigen::Ref & c, const Eigen::Ref & d) +{ + const auto n = d.size(); + + if (n == 1) { + return d.array() / b.array(); + } + + Eigen::VectorXd c_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd d_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd x = Eigen::VectorXd::Zero(n); + + // Forward sweep + c_prime(0) = c(0) / b(0); + d_prime(0) = d(0) / b(0); + + for (auto i = 1; i < n; i++) { + const double m = 1.0 / (b(i) - a(i - 1) * c_prime(i - 1)); + c_prime(i) = i < n - 1 ? c(i) * m : 0; + d_prime(i) = (d(i) - a(i - 1) * d_prime(i - 1)) * m; + } + + // Back substitution + x(n - 1) = d_prime(n - 1); + + for (int64_t i = n - 2; i >= 0; i--) { + x(i) = d_prime(i) - c_prime(i) * x(i + 1); + } + + return x; +} + +std::vector spline( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + // calculate spline coefficients + SplineInterpolation interpolator(base_keys, base_values); + + // interpolate base_keys at query_keys + return interpolator.getSplineInterpolatedValues(query_keys); +} + +std::vector splineByAkima( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + constexpr double epsilon = 1e-5; + + // calculate m + std::vector m_values; + for (size_t i = 0; i < base_keys.size() - 1; ++i) { + const double m_val = + (base_values.at(i + 1) - base_values.at(i)) / (base_keys.at(i + 1) - base_keys.at(i)); + m_values.push_back(m_val); + } + + // calculate s + std::vector s_values; + for (size_t i = 0; i < base_keys.size(); ++i) { + if (i == 0) { + s_values.push_back(m_values.front()); + continue; + } else if (i == base_keys.size() - 1) { + s_values.push_back(m_values.back()); + continue; + } else if (i == 1 || i == base_keys.size() - 2) { + const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0; + s_values.push_back(s_val); + continue; + } + + const double denom = std::abs(m_values.at(i + 1) - m_values.at(i)) + + std::abs(m_values.at(i - 1) - m_values.at(i - 2)); + if (std::abs(denom) < epsilon) { + const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0; + s_values.push_back(s_val); + continue; + } + + const double s_val = (std::abs(m_values.at(i + 1) - m_values.at(i)) * m_values.at(i - 1) + + std::abs(m_values.at(i - 1) - m_values.at(i - 2)) * m_values.at(i)) / + denom; + s_values.push_back(s_val); + } + + // calculate cubic coefficients + std::vector a; + std::vector b; + std::vector c; + std::vector d; + for (size_t i = 0; i < base_keys.size() - 1; ++i) { + a.push_back( + (s_values.at(i) + s_values.at(i + 1) - 2.0 * m_values.at(i)) / + std::pow(base_keys.at(i + 1) - base_keys.at(i), 2)); + b.push_back( + (3.0 * m_values.at(i) - 2.0 * s_values.at(i) - s_values.at(i + 1)) / + (base_keys.at(i + 1) - base_keys.at(i))); + c.push_back(s_values.at(i)); + d.push_back(base_values.at(i)); + } + + // interpolate + std::vector res; + size_t j = 0; + for (const auto & query_key : query_keys) { + while (base_keys.at(j + 1) < query_key) { + ++j; + } + + const double ds = query_key - base_keys.at(j); + res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); + } + return res; +} + +Eigen::Index SplineInterpolation::get_index(const double & key) const +{ + const auto it = std::lower_bound(base_keys_.begin(), base_keys_.end(), key); + return std::clamp( + static_cast(std::distance(base_keys_.begin(), it)) - 1, 0, + static_cast(base_keys_.size()) - 2); +} + +void SplineInterpolation::calcSplineCoefficients( + const std::vector & base_keys, const std::vector & base_values) +{ + // throw exceptions for invalid arguments + autoware::interpolation::validateKeysAndValues(base_keys, base_values); + const Eigen::VectorXd x = Eigen::Map( + base_keys.data(), static_cast(base_keys.size())); + const Eigen::VectorXd y = Eigen::Map( + base_values.data(), static_cast(base_values.size())); + + const auto n = x.size(); + + if (n == 2) { + a_ = Eigen::VectorXd::Zero(1); + b_ = Eigen::VectorXd::Zero(1); + c_ = Eigen::VectorXd::Zero(1); + d_ = Eigen::VectorXd::Zero(1); + c_[0] = (y[1] - y[0]) / (x[1] - x[0]); + d_[0] = y[0]; + base_keys_ = base_keys; + return; + } + + // Create Tridiagonal matrix + Eigen::VectorXd v(n); + const Eigen::VectorXd h = x.segment(1, n - 1) - x.segment(0, n - 1); + const Eigen::VectorXd a = h.segment(1, n - 3); + const Eigen::VectorXd b = 2 * (h.segment(0, n - 2) + h.segment(1, n - 2)); + const Eigen::VectorXd c = h.segment(1, n - 3); + const Eigen::VectorXd y_diff = y.segment(1, n - 1) - y.segment(0, n - 1); + const Eigen::VectorXd d = 6 * (y_diff.segment(1, n - 2).array() / h.tail(n - 2).array() - + y_diff.segment(0, n - 2).array() / h.head(n - 2).array()); + + // Solve tridiagonal matrix + v.segment(1, n - 2) = solve_tridiagonal_matrix_algorithm(a, b, c, d); + v[0] = 0; + v[n - 1] = 0; + + // Calculate spline coefficients + a_ = (v.tail(n - 1) - v.head(n - 1)).array() / 6.0 / (x.tail(n - 1) - x.head(n - 1)).array(); + b_ = v.segment(0, n - 1) / 2.0; + c_ = (y.tail(n - 1) - y.head(n - 1)).array() / (x.tail(n - 1) - x.head(n - 1)).array() - + (x.tail(n - 1) - x.head(n - 1)).array() * + (2 * v.segment(0, n - 1).array() + v.segment(1, n - 1).array()) / 6.0; + d_ = y.head(n - 1); + base_keys_ = base_keys; +} + +std::vector SplineInterpolation::getSplineInterpolatedValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); + std::vector interpolated_values; + interpolated_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_values.emplace_back( + a_[idx] * dx * dx * dx + b_[idx] * dx * dx + c_[idx] * dx + d_[idx]); + } + + return interpolated_values; +} + +std::vector SplineInterpolation::getSplineInterpolatedDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); + std::vector interpolated_diff_values; + interpolated_diff_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_diff_values.emplace_back(3 * a_[idx] * dx * dx + 2 * b_[idx] * dx + c_[idx]); + } + + return interpolated_diff_values; +} + +std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); + std::vector interpolated_quad_diff_values; + interpolated_quad_diff_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_quad_diff_values.emplace_back(6 * a_[idx] * dx + 2 * b_[idx]); + } + + return interpolated_quad_diff_values; +} +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp similarity index 97% rename from common/interpolation/src/spline_interpolation_points_2d.cpp rename to common/autoware_interpolation/src/spline_interpolation_points_2d.cpp index 95fde68b171bd..e0f55cfb24ba8 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include -namespace +namespace autoware::interpolation { std::vector calcEuclidDist(const std::vector & x, const std::vector & y) { @@ -66,10 +66,6 @@ std::array, 4> getBaseValues( return {base_s, base_x, base_y, base_z}; } -} // namespace - -namespace interpolation -{ template std::vector splineYawFromPoints(const std::vector & points) @@ -88,8 +84,6 @@ std::vector splineYawFromPoints(const std::vector & points) template std::vector splineYawFromPoints( const std::vector & points); -} // namespace interpolation - geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( const size_t idx, const double s) const { @@ -215,3 +209,4 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec); } +} // namespace autoware::interpolation diff --git a/common/interpolation/test/src/test_interpolation.cpp b/common/autoware_interpolation/test/src/test_interpolation.cpp similarity index 100% rename from common/interpolation/test/src/test_interpolation.cpp rename to common/autoware_interpolation/test/src/test_interpolation.cpp diff --git a/common/interpolation/test/src/test_interpolation_utils.cpp b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp similarity index 85% rename from common/interpolation/test/src/test_interpolation_utils.cpp rename to common/autoware_interpolation/test/src/test_interpolation_utils.cpp index 8b3a3b9faa0c6..2aa41b6fdef00 100644 --- a/common/interpolation/test/src/test_interpolation_utils.cpp +++ b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include @@ -24,43 +24,43 @@ TEST(interpolation_utils, isIncreasing) { // empty const std::vector empty_vec; - EXPECT_THROW(interpolation_utils::isIncreasing(empty_vec), std::invalid_argument); + EXPECT_THROW(autoware::interpolation::isIncreasing(empty_vec), std::invalid_argument); // increase const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isIncreasing(increasing_vec), true); // not decrease const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(not_increasing_vec), false); + EXPECT_EQ(autoware::interpolation::isIncreasing(not_increasing_vec), false); // decrease const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(decreasing_vec), false); + EXPECT_EQ(autoware::interpolation::isIncreasing(decreasing_vec), false); } TEST(interpolation_utils, isNotDecreasing) { // empty const std::vector empty_vec; - EXPECT_THROW(interpolation_utils::isNotDecreasing(empty_vec), std::invalid_argument); + EXPECT_THROW(autoware::interpolation::isNotDecreasing(empty_vec), std::invalid_argument); // increase const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(increasing_vec), true); // not decrease const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(not_increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(not_increasing_vec), true); // decrease const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(decreasing_vec), false); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(decreasing_vec), false); } TEST(interpolation_utils, validateKeys) { - using interpolation_utils::validateKeys; + using autoware::interpolation::validateKeys; const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; const std::vector query_keys{0.0, 1.0, 2.0, 3.0}; @@ -116,7 +116,7 @@ TEST(interpolation_utils, validateKeys) TEST(interpolation_utils, validateKeysAndValues) { - using interpolation_utils::validateKeysAndValues; + using autoware::interpolation::validateKeysAndValues; const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; const std::vector base_values{0.0, 1.0, 2.0, 3.0}; diff --git a/common/interpolation/test/src/test_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp similarity index 80% rename from common/interpolation/test/src/test_linear_interpolation.cpp rename to common/autoware_interpolation/test/src/test_linear_interpolation.cpp index 9c392943bd3c5..0fea1e514e916 100644 --- a/common/interpolation/test/src/test_linear_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include @@ -23,8 +23,8 @@ constexpr double epsilon = 1e-6; TEST(linear_interpolation, lerp_scalar) { - EXPECT_EQ(interpolation::lerp(0.0, 1.0, 0.3), 0.3); - EXPECT_EQ(interpolation::lerp(-0.5, 12.3, 0.3), 3.34); + EXPECT_EQ(autoware::interpolation::lerp(0.0, 1.0, 0.3), 0.3); + EXPECT_EQ(autoware::interpolation::lerp(-0.5, 12.3, 0.3), 3.34); } TEST(linear_interpolation, lerp_vector) @@ -35,7 +35,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -47,7 +47,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -59,7 +59,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -71,7 +71,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.18, 1.12, 1.4}; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -87,7 +87,8 @@ TEST(linear_interpolation, lerp_scalar_query) const std::vector ans{-0.18, 1.12, 1.4}; for (size_t i = 0; i < query_keys.size(); ++i) { - const auto query_value = interpolation::lerp(base_keys, base_values, query_keys.at(i)); + const auto query_value = + autoware::interpolation::lerp(base_keys, base_values, query_keys.at(i)); EXPECT_NEAR(query_value, ans.at(i), epsilon); } } diff --git a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp similarity index 96% rename from common/interpolation/test/src/test_spherical_linear_interpolation.cpp rename to common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp index 0bb9ba8ef2ce8..b7ce134c680bd 100644 --- a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include @@ -34,7 +34,7 @@ inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( TEST(slerp, spline_scalar) { - using interpolation::slerp; + using autoware::interpolation::slerp; // Same value { @@ -79,7 +79,7 @@ TEST(slerp, spline_scalar) TEST(slerp, spline_vector) { - using interpolation::slerp; + using autoware::interpolation::slerp; // query keys are same as base keys { diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp similarity index 81% rename from common/interpolation/test/src/test_spline_interpolation.cpp rename to common/autoware_interpolation/test/src/test_spline_interpolation.cpp index d3cb2d6d3060b..766e94a47b968 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/spline_interpolation.hpp" #include @@ -22,6 +22,8 @@ constexpr double epsilon = 1e-6; +using autoware::interpolation::SplineInterpolation; + TEST(spline_interpolation, spline) { { // straight: query_keys is same as base_keys @@ -30,7 +32,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -42,7 +44,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -54,7 +56,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -64,9 +66,9 @@ TEST(spline_interpolation, spline) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -78,7 +80,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -90,7 +92,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -102,7 +104,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.808769, -0.077539, 1.035096}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -112,9 +114,9 @@ TEST(spline_interpolation, spline) const std::vector base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0}; const std::vector base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; - const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; + const std::vector ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -129,7 +131,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -141,7 +144,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -153,7 +157,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -165,7 +170,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.0801, 1.110749, 1.4864}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -177,7 +183,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -189,7 +196,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -201,7 +209,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.8378, -0.0801, 0.927031}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -213,7 +222,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -227,7 +237,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); @@ -242,7 +252,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; - const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; + const std::vector ans{0.671343, 0.049289, 0.209471, -0.253746}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); @@ -257,7 +267,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; - const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; + const std::vector ans{-0.155829, 0.043097, -0.011143, -0.049611}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp similarity index 84% rename from common/interpolation/test/src/test_spline_interpolation_points_2d.cpp rename to common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp index 4013832220cd8..974ad606b978d 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include @@ -23,6 +23,8 @@ constexpr double epsilon = 1e-6; +using autoware::interpolation::SplineInterpolationPoints2d; + TEST(spline_interpolation, splineYawFromPoints) { using autoware::universe_utils::createPoint; @@ -37,7 +39,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -51,9 +53,8 @@ TEST(spline_interpolation, splineYawFromPoints) points.push_back(createPoint(5.0, 10.0, 0.0)); points.push_back(createPoint(10.0, 12.5, 0.0)); - const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - - const auto yaws = interpolation::splineYawFromPoints(points); + const std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -63,7 +64,7 @@ TEST(spline_interpolation, splineYawFromPoints) std::vector points; points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); + EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) @@ -73,7 +74,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -87,7 +88,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -121,8 +122,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // random const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.3036484, epsilon); - EXPECT_NEAR(random_point.y, 10.3343074, epsilon); + EXPECT_NEAR(random_point.x, 5.28974, epsilon); + EXPECT_NEAR(random_point.y, 10.3450319, epsilon); // out of range of total length const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); @@ -140,17 +141,17 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) { // yaw // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon); // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.808580, epsilon); // out of range of total length - EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.368174, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.278594, epsilon); // out of range of index EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); @@ -165,7 +166,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon); // out of range of total length EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); diff --git a/common/interpolation/test/src/test_zero_order_hold.cpp b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp similarity index 80% rename from common/interpolation/test/src/test_zero_order_hold.cpp rename to common/autoware_interpolation/test/src/test_zero_order_hold.cpp index 541af1cf76064..c99348d66d034 100644 --- a/common/interpolation/test/src/test_zero_order_hold.cpp +++ b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/zero_order_hold.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include @@ -29,7 +29,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -41,7 +42,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 0.0, 1.5, 6.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -53,7 +55,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -65,7 +68,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-1.2, 1.0, 2.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -77,7 +81,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001}; const std::vector ans = {0.0, 1.5, 2.5, 3.5, 3.5}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -89,7 +94,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001}; const std::vector ans = {0.0, 1.5, 2.5, 3.5, 0.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -104,7 +110,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys = base_keys; const auto ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_EQ(query_values.at(i), ans.at(i)); } @@ -116,7 +123,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans = {true, true, false, false}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_EQ(query_values.at(i), ans.at(i)); } @@ -128,7 +136,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001}; const std::vector ans = {true, true, false, true, true}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -140,7 +149,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001}; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index e7f1f56a451f4..8cae2a8ac4110 100644 --- a/common/autoware_motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -7,16 +7,11 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -find_package(fmt REQUIRED) ament_auto_add_library(autoware_motion_utils SHARED DIRECTORY src ) -target_link_libraries(autoware_motion_utils - fmt::fmt -) - if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) @@ -45,6 +40,7 @@ if(BUILD_EXAMPLES) foreach(example_file ${example_files}) get_filename_component(example_name ${example_file} NAME_WE) ament_auto_add_executable(${example_name} ${example_file}) + set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter) target_link_libraries(${example_name} autoware_motion_utils matplotlibcpp17::matplotlibcpp17 diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp index 8c98143af7e28..6ce81e4572276 100644 --- a/common/autoware_motion_utils/examples/example_interpolator.cpp +++ b/common/autoware_motion_utils/examples/example_interpolator.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" #include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp" #include @@ -27,26 +31,25 @@ int main() auto plt = matplotlibcpp17::pyplot::import(); // create random values - std::vector axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; + std::vector bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; std::vector values; std::random_device seed_gen; std::mt19937 engine(seed_gen()); std::uniform_real_distribution<> dist(-1.0, 1.0); - for (size_t i = 0; i < axis.size(); ++i) { + for (size_t i = 0; i < bases.size(); ++i) { values.push_back(dist(engine)); } // Scatter Data - plt.scatter(Args(axis, values)); + plt.scatter(Args(bases, values)); - using autoware::motion_utils::trajectory_container::interpolator::Interpolator; - using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; + using autoware::motion_utils::trajectory_container::interpolator::InterpolatorInterface; // Linear Interpolator { using autoware::motion_utils::trajectory_container::interpolator::Linear; - auto interpolator = *InterpolatorCreator().set_axis(axis).set_values(values).create(); + auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -56,11 +59,11 @@ int main() // AkimaSpline Interpolator { using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline; - auto interpolator = - *InterpolatorCreator().set_axis(axis).set_values(values).create(); + + auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -70,11 +73,10 @@ int main() // CubicSpline Interpolator { using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; - auto interpolator = - *InterpolatorCreator().set_axis(axis).set_values(values).create(); + auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -85,10 +87,10 @@ int main() { using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor; auto interpolator = - *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + *NearestNeighbor::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -98,11 +100,10 @@ int main() // Stairstep Interpolator { using autoware::motion_utils::trajectory_container::interpolator::Stairstep; - auto interpolator = - *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + auto interpolator = *Stairstep::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp index a6542d1fc212f..a6a0c69744301 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ #include #include -#include #include #include #include diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp index 363d046534399..f9a2ac1316a4e 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator @@ -30,7 +29,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform Akima spline interpolation on a set of data points. */ -class AkimaSpline : public Interpolator +class AkimaSpline : public detail::InterpolatorMixin { private: Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. @@ -40,21 +39,20 @@ class AkimaSpline : public Interpolator * * This method computes the coefficients for the Akima spline. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ void compute_parameters( - const Eigen::Ref & axis, + const Eigen::Ref & bases, const Eigen::Ref & values); /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -89,13 +87,6 @@ class AkimaSpline : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override { return 5; } - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp index 50cff1dde3f35..437ddd727cc7d 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include @@ -29,33 +29,32 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform cubic spline interpolation on a set of data points. */ -class CubicSpline : public Interpolator +class CubicSpline : public detail::InterpolatorMixin { private: Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the cubic spline. - Eigen::VectorXd h_; ///< Interval sizes between axis points. + Eigen::VectorXd h_; ///< Interval sizes between bases points. /** * @brief Compute the spline parameters. * * This method computes the coefficients for the cubic spline. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ void compute_parameters( - const Eigen::Ref & axis, + const Eigen::Ref & bases, const Eigen::Ref & values); /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -90,13 +89,6 @@ class CubicSpline : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override { return 4; } - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp similarity index 77% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp index f7de456b736a4..f0131e85e157c 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp @@ -13,15 +13,13 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT // clang-format on #include #include -#include - #include namespace autoware::motion_utils::trajectory_container::interpolator::detail @@ -35,20 +33,20 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class InterpolatorCommonImpl +class InterpolatorCommonInterface { protected: - Eigen::VectorXd axis_; ///< Axis values for the interpolation. + std::vector bases_; ///< bases values for the interpolation. /** * @brief Get the start of the interpolation range. */ - [[nodiscard]] double start() const { return axis_(0); } + [[nodiscard]] double start() const { return bases_.front(); } /** * @brief Get the end of the interpolation range. */ - [[nodiscard]] double end() const { return axis_(axis_.size() - 1); } + [[nodiscard]] double end() const { return bases_.back(); } /** * @brief Compute the interpolated value at the given point. @@ -65,11 +63,10 @@ class InterpolatorCommonImpl * * This method should be overridden by subclasses to provide the specific build logic. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ - virtual void build_impl( - const Eigen::Ref & axis, const std::vector & values) = 0; + virtual void build_impl(const std::vector & bases, const std::vector & values) = 0; /** * @brief Validate the input to the compute method. @@ -91,29 +88,30 @@ class InterpolatorCommonImpl [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const { if (end_inclusive && s == end()) { - return static_cast(axis_.size()) - 2; + return static_cast(bases_.size()) - 2; } auto comp = [](const double & a, const double & b) { return a <= b; }; - return std::distance(axis_.begin(), std::lower_bound(axis_.begin(), axis_.end(), s, comp)) - 1; + return std::distance(bases_.begin(), std::lower_bound(bases_.begin(), bases_.end(), s, comp)) - + 1; } public: /** - * @brief Build the interpolator with the given axis and values. + * @brief Build the interpolator with the given bases and values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - bool build(const Eigen::Ref & axis, const std::vector & values) + bool build(const std::vector & bases, const std::vector & values) { - if (axis.size() != static_cast(values.size())) { + if (bases.size() != values.size()) { return false; } - if (axis.size() < static_cast(minimum_required_points())) { + if (bases.size() < minimum_required_points()) { return false; } - build_impl(axis, values); + build_impl(bases, values); return true; } @@ -141,5 +139,5 @@ class InterpolatorCommonImpl } // namespace autoware::motion_utils::trajectory_container::interpolator::detail // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp new file mode 100644 index 0000000000000..d3957567c49b5 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ + +/** + * @brief Base class for interpolator implementations. + * + * This class implements the core functionality for interpolator implementations. + * + * @tparam InterpolatorType The type of the interpolator implementation. + * @tparam T The type of the values being interpolated. + */ +template +struct InterpolatorMixin : public InterpolatorInterface +{ + std::shared_ptr> clone() const override + { + return std::make_shared(static_cast(*this)); + } + + class Builder + { + private: + std::vector bases_; + std::vector values_; + + public: + [[nodiscard]] Builder & set_bases(const Eigen::Ref & bases) + { + bases_ = std::vector(bases.begin(), bases.end()); + return *this; + } + + [[nodiscard]] Builder & set_bases(const std::vector & bases) + { + bases_ = bases; + return *this; + } + + [[nodiscard]] Builder & set_values(const std::vector & values) + { + values_ = values; + return *this; + } + + [[nodiscard]] Builder & set_values(const Eigen::Ref & values) + { + values_ = std::vector(values.begin(), values.end()); + return *this; + } + + template + [[nodiscard]] std::optional build(Args &&... args) + { + auto interpolator = InterpolatorType(std::forward(args)...); + bool success = interpolator.build(bases_, values_); + if (!success) { + return std::nullopt; + } + return interpolator; + } + }; +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp index 9fd451bcdcf6e..ad0e2b98ef040 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -17,11 +17,17 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +template +class NearestNeighbor; + +namespace detail { /** * @brief Common Implementation of nearest neighbor. @@ -31,7 +37,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class NearestNeighborCommonImpl : public Interpolator +class NearestNeighborCommonImpl : public detail::InterpolatorMixin, T> { protected: std::vector values_; ///< Interpolation values. @@ -45,7 +51,7 @@ class NearestNeighborCommonImpl : public Interpolator [[nodiscard]] T compute_impl(const double & s) const override { const int32_t idx = this->get_index(s); - return (std::abs(s - this->axis_[idx]) <= std::abs(s - this->axis_[idx + 1])) + return (std::abs(s - this->bases_[idx]) <= std::abs(s - this->bases_[idx + 1])) ? this->values_.at(idx) : this->values_.at(idx + 1); } @@ -53,14 +59,13 @@ class NearestNeighborCommonImpl : public Interpolator /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override + void build_impl(const std::vector & bases, const std::vector & values) override { - this->axis_ = axis; + this->bases_ = bases; this->values_ = values; } @@ -73,8 +78,8 @@ class NearestNeighborCommonImpl : public Interpolator [[nodiscard]] size_t minimum_required_points() const override { return 1; } }; -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail - +} // namespace detail +} // namespace autoware::motion_utils::trajectory_container::interpolator // clang-format off #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp index 9ac5282429353..83b2275b0c121 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp @@ -17,11 +17,17 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +template +class Stairstep; + +namespace detail { /** @@ -32,7 +38,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class StairstepCommonImpl : public Interpolator +class StairstepCommonImpl : public detail::InterpolatorMixin, T> { protected: std::vector values_; ///< Interpolation values. @@ -51,13 +57,12 @@ class StairstepCommonImpl : public Interpolator /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override + void build_impl(const std::vector & bases, const std::vector & values) override { - this->axis_ = axis; + this->bases_ = bases; this->values_ = values; } @@ -72,8 +77,8 @@ class StairstepCommonImpl : public Interpolator */ [[nodiscard]] size_t minimum_required_points() const override { return 2; } }; - -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail +} // namespace detail +} // namespace autoware::motion_utils::trajectory_container::interpolator // clang-format off #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp index f0b12a47819e8..03827ded59cb6 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp" #include @@ -29,10 +29,10 @@ namespace autoware::motion_utils::trajectory_container::interpolator * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) */ template -class Interpolator : public detail::InterpolatorCommonImpl +class InterpolatorInterface : public detail::InterpolatorCommonInterface { public: - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; + [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; /** @@ -41,7 +41,7 @@ class Interpolator : public detail::InterpolatorCommonImpl * This class adds methods for computing first and second derivatives. */ template <> -class Interpolator : public detail::InterpolatorCommonImpl +class InterpolatorInterface : public detail::InterpolatorCommonInterface { protected: /** @@ -89,7 +89,7 @@ class Interpolator : public detail::InterpolatorCommonImpl return compute_second_derivative_impl(s); } - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; + [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp deleted file mode 100644 index 911adcb6545b6..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2024 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 AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ - -#include - -#include -#include -#include - -namespace autoware::motion_utils::trajectory_container::interpolator -{ - -// Forward declaration -template -class Interpolator; - -template -class InterpolatorCreator -{ -private: - Eigen::VectorXd axis_; - std::vector values_; - -public: - [[nodiscard]] InterpolatorCreator & set_axis(const Eigen::Ref & axis) - { - axis_ = axis; - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_axis(const std::vector & axis) - { - axis_ = Eigen::Map(axis.data(), static_cast(axis.size())); - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_values(const std::vector & values) - { - values_ = values; - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_values(const Eigen::Ref & values) - { - values_ = std::vector(values.begin(), values.end()); - return *this; - } - - template - [[nodiscard]] std::optional create(Args &&... args) - { - auto interpolator = InterpolatorType(std::forward(args)...); - bool success = interpolator.build(axis_, values_); - if (!success) { - return std::nullopt; - } - return interpolator; - } -}; - -} // namespace autoware::motion_utils::trajectory_container::interpolator - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp index 9854b100f3742..99fcb469365b8 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator @@ -30,7 +29,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform linear interpolation on a set of data points. */ -class Linear : public Interpolator +class Linear : public detail::InterpolatorMixin { private: Eigen::VectorXd values_; ///< Interpolation values. @@ -38,12 +37,11 @@ class Linear : public Interpolator /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -81,13 +79,6 @@ class Linear : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to the cloned interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp index 32bff2d30b41d..72b61001b05e3 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp @@ -17,8 +17,6 @@ #include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" -#include - namespace autoware::motion_utils::trajectory_container::interpolator { @@ -44,16 +42,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl { public: NearestNeighbor() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; /** @@ -83,16 +71,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl public: NearestNeighbor() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp index 21bf57b46e3b7..801b207b25afc 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp @@ -17,8 +17,6 @@ #include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" -#include - namespace autoware::motion_utils::trajectory_container::interpolator { @@ -44,16 +42,6 @@ class Stairstep : public detail::StairstepCommonImpl { public: Stairstep() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; /** @@ -83,16 +71,6 @@ class Stairstep : public detail::StairstepCommonImpl public: Stairstep() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 81fec78b04812..1fe2c4d8e1ea8 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -22,12 +22,12 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_interpolation autoware_planning_msgs autoware_universe_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs - interpolation libboost-dev rclcpp tf2 diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 1f29a4577e428..88ebf3f41c408 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -14,12 +14,12 @@ #include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/resample/resample_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" #include @@ -61,13 +61,13 @@ std::vector resamplePointVector( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; const auto spline = [&](const auto & input) { - return interpolation::spline(input_arclength, input, resampled_arclength); + return autoware::interpolation::spline(input_arclength, input, resampled_arclength); }; const auto spline_by_akima = [&](const auto & input) { - return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + return autoware::interpolation::splineByAkima(input_arclength, input, resampled_arclength); }; const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); @@ -280,14 +280,15 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampling_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampling_arclength); }; auto closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -462,16 +463,17 @@ autoware_planning_msgs::msg::Path resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; std::vector closest_segment_indices; if (use_zero_order_hold_for_v) { closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -636,16 +638,17 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; std::vector closest_segment_indices; if (use_zero_order_hold_for_twist) { closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 4a9eaeca58d30..0ae9d44683f62 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/linear_interpolation.hpp" using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -66,26 +66,26 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; } else { - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = - interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation - interpolated_point.heading_rate_rps = - interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation - interpolated_point.front_wheel_angle_rad = interpolation::lerp( + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = - interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation - const double interpolated_time = interpolation::lerp( + const double interpolated_time = autoware::interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); @@ -134,15 +134,15 @@ PathPointWithLaneId calcInterpolatedPoint( interpolated_point.point.longitudinal_velocity_mps = curr_pt.point.longitudinal_velocity_mps; interpolated_point.point.lateral_velocity_mps = curr_pt.point.lateral_velocity_mps; } else { - interpolated_point.point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.point.longitudinal_velocity_mps, next_pt.point.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.point.lateral_velocity_mps, next_pt.point.lateral_velocity_mps, clamped_ratio); } // heading rate interpolation - interpolated_point.point.heading_rate_rps = interpolation::lerp( + interpolated_point.point.heading_rate_rps = autoware::interpolation::lerp( curr_pt.point.heading_rate_rps, next_pt.point.heading_rate_rps, clamped_ratio); return interpolated_point; diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index cd8de63f56c1d..55c7360a25c41 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include #include @@ -106,7 +106,7 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( auto cog_path = path; // calculate curvature and yaw from spline interpolation - const auto spline = SplineInterpolationPoints2d(path.points); + const auto spline = autoware::interpolation::SplineInterpolationPoints2d(path.points); const auto curvature_vec = spline.getSplineInterpolatedCurvatures(); const auto yaw_vec = spline.getSplineInterpolatedYaws(); diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp index eb2fe2ab8deed..747362ac9167e 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp @@ -17,18 +17,17 @@ #include #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator { void AkimaSpline::compute_parameters( - const Eigen::Ref & axis, const Eigen::Ref & values) + const Eigen::Ref & bases, const Eigen::Ref & values) { - const auto n = static_cast(axis.size()); + const auto n = static_cast(bases.size()); - Eigen::VectorXd h = axis.tail(n - 1) - axis.head(n - 1); + Eigen::VectorXd h = bases.tail(n - 1) - bases.head(n - 1); Eigen::VectorXd m(n - 1); for (int32_t i = 0; i < n - 1; ++i) { @@ -61,39 +60,33 @@ void AkimaSpline::compute_parameters( } } -void AkimaSpline::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void AkimaSpline::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; compute_parameters( - this->axis_, + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); } double AkimaSpline::compute_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return a_[i] + b_[i] * dx + c_[i] * dx * dx + d_[i] * dx * dx * dx; } double AkimaSpline::compute_first_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return b_[i] + 2 * c_[i] * dx + 3 * d_[i] * dx * dx; } double AkimaSpline::compute_second_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return 2 * c_[i] + 6 * d_[i] * dx; } -std::shared_ptr> AkimaSpline::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp index db25816d1d457..e464fc7cd0511 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp @@ -20,15 +20,15 @@ namespace autoware::motion_utils::trajectory_container::interpolator { void CubicSpline::compute_parameters( - const Eigen::Ref & axis, const Eigen::Ref & values) + const Eigen::Ref & bases, const Eigen::Ref & values) { - const int32_t n = static_cast(axis.size()) - 1; + const int32_t n = static_cast(bases.size()) - 1; - h_ = axis.tail(n) - axis.head(n); + h_ = bases.tail(n) - bases.head(n); a_ = values.transpose(); for (int32_t i = 0; i < n; ++i) { - h_(i) = axis(i + 1) - axis(i); + h_(i) = bases(i + 1) - bases(i); } Eigen::VectorXd alpha(n - 1); @@ -43,7 +43,7 @@ void CubicSpline::compute_parameters( mu(0) = z(0) = 0.0; for (int32_t i = 1; i < n; ++i) { - l(i) = 2.0 * (axis(i + 1) - axis(i - 1)) - h_(i - 1) * mu(i - 1); + l(i) = 2.0 * (bases(i + 1) - bases(i - 1)) - h_(i - 1) * mu(i - 1); mu(i) = h_(i) / l(i); z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); } @@ -61,39 +61,33 @@ void CubicSpline::compute_parameters( } } -void CubicSpline::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void CubicSpline::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; compute_parameters( - this->axis_, + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); } double CubicSpline::compute_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return a_(i) + b_(i) * dx + c_(i) * dx * dx + d_(i) * dx * dx * dx; } double CubicSpline::compute_first_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return b_(i) + 2 * c_(i) * dx + 3 * d_(i) * dx * dx; } double CubicSpline::compute_second_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return 2 * c_(i) + 6 * d_(i) * dx; } -std::shared_ptr> CubicSpline::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp index c3e4ec99c4357..fb714b208772c 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp @@ -16,16 +16,14 @@ #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator { -void Linear::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void Linear::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; this->values_ = Eigen::Map(values.data(), static_cast(values.size())); } @@ -33,8 +31,8 @@ void Linear::build_impl( double Linear::compute_impl(const double & s) const { const int32_t idx = this->get_index(s); - const double x0 = this->axis_(idx); - const double x1 = this->axis_(idx + 1); + const double x0 = this->bases_.at(idx); + const double x1 = this->bases_.at(idx + 1); const double y0 = this->values_(idx); const double y1 = this->values_(idx + 1); return y0 + (y1 - y0) * (s - x0) / (x1 - x0); @@ -43,8 +41,8 @@ double Linear::compute_impl(const double & s) const double Linear::compute_first_derivative_impl(const double & s) const { const int32_t idx = this->get_index(s); - const double x0 = this->axis_(idx); - const double x1 = this->axis_(idx + 1); + const double x0 = this->bases_.at(idx); + const double x1 = this->bases_.at(idx + 1); const double y0 = this->values_(idx); const double y1 = this->values_(idx + 1); return (y1 - y0) / (x1 - x0); @@ -59,10 +57,4 @@ size_t Linear::minimum_required_points() const { return 2; } - -std::shared_ptr> Linear::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp index 77f7af0eae93f..82c387c416436 100644 --- a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include @@ -26,7 +25,7 @@ class TestInterpolator : public ::testing::Test { public: std::optional interpolator; - std::vector axis; + std::vector bases; std::vector values; void SetUp() override @@ -35,10 +34,10 @@ class TestInterpolator : public ::testing::Test std::random_device rd; std::mt19937 gen(rd()); std::uniform_real_distribution<> dis(-1, 1); - axis.resize(10); + bases.resize(10); values.resize(10); - for (size_t i = 0; i < axis.size(); ++i) { - axis[i] = static_cast(i); + for (size_t i = 0; i < bases.size(); ++i) { + bases[i] = static_cast(i); values[i] = dis(gen); } } @@ -55,11 +54,10 @@ TYPED_TEST_SUITE(TestInterpolator, Interpolators, ); TYPED_TEST(TestInterpolator, compute) { - using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; this->interpolator = - InterpolatorCreator().set_axis(this->axis).set_values(this->values).create(); - for (size_t i = 0; i < this->axis.size(); ++i) { - EXPECT_NEAR(this->values[i], this->interpolator->compute(this->axis[i]), 1e-6); + typename TypeParam::Builder().set_bases(this->bases).set_values(this->values).build(); + for (size_t i = 0; i < this->bases.size(); ++i) { + EXPECT_NEAR(this->values[i], this->interpolator->compute(this->bases[i]), 1e-6); } } diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 8670f85e6ba71..d677be40dead2 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -56,7 +56,7 @@ void SpeedDisplay::updateSpeedData( // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in float speed = msg->longitudinal_velocity; // we received it as a m/s value, but we want to display it in km/h - current_speed_ = (speed * 3.6); + current_speed_ = std::abs(speed * 3.6); } catch (const std::exception & e) { // Log the error std::cerr << "Error in processMessage: " << e.what() << std::endl; diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 28ff67433efe5..04d07462e8695 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -538,6 +538,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( return marker_ptr; } +// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, diff --git a/common/signal_processing/CMakeLists.txt b/common/autoware_signal_processing/CMakeLists.txt similarity index 94% rename from common/signal_processing/CMakeLists.txt rename to common/autoware_signal_processing/CMakeLists.txt index 89f14d4339223..7e6a0429fe93f 100644 --- a/common/signal_processing/CMakeLists.txt +++ b/common/autoware_signal_processing/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(signal_processing) +project(autoware_signal_processing) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/common/signal_processing/README.md b/common/autoware_signal_processing/README.md similarity index 100% rename from common/signal_processing/README.md rename to common/autoware_signal_processing/README.md diff --git a/common/signal_processing/documentation/ButterworthFilter.md b/common/autoware_signal_processing/documentation/ButterworthFilter.md similarity index 100% rename from common/signal_processing/documentation/ButterworthFilter.md rename to common/autoware_signal_processing/documentation/ButterworthFilter.md diff --git a/common/signal_processing/documentation/img.png b/common/autoware_signal_processing/documentation/img.png similarity index 100% rename from common/signal_processing/documentation/img.png rename to common/autoware_signal_processing/documentation/img.png diff --git a/common/signal_processing/include/signal_processing/butterworth.hpp b/common/autoware_signal_processing/include/autoware/signal_processing/butterworth.hpp similarity index 94% rename from common/signal_processing/include/signal_processing/butterworth.hpp rename to common/autoware_signal_processing/include/autoware/signal_processing/butterworth.hpp index 579a915cc9968..11099aca1f206 100644 --- a/common/signal_processing/include/signal_processing/butterworth.hpp +++ b/common/autoware_signal_processing/include/autoware/signal_processing/butterworth.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIGNAL_PROCESSING__BUTTERWORTH_HPP_ -#define SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#ifndef AUTOWARE__SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#define AUTOWARE__SIGNAL_PROCESSING__BUTTERWORTH_HPP_ #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::signal_processing +{ + template const T & append_separator(const T & arg) { @@ -133,5 +136,6 @@ class ButterworthFilter // Computes continuous time roots from the phase angles void computeContinuousTimeRoots(bool const & use_sampling_frequency = false); }; +} // namespace autoware::signal_processing -#endif // SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#endif // AUTOWARE__SIGNAL_PROCESSING__BUTTERWORTH_HPP_ diff --git a/common/signal_processing/include/signal_processing/lowpass_filter.hpp b/common/autoware_signal_processing/include/autoware/signal_processing/lowpass_filter.hpp similarity index 82% rename from common/signal_processing/include/signal_processing/lowpass_filter.hpp rename to common/autoware_signal_processing/include/autoware/signal_processing/lowpass_filter.hpp index 89b7a6da263f7..f09e1dda461da 100644 --- a/common/signal_processing/include/signal_processing/lowpass_filter.hpp +++ b/common/autoware_signal_processing/include/autoware/signal_processing/lowpass_filter.hpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ -#define SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE__SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ +#define AUTOWARE__SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "geometry_msgs/msg/twist.hpp" +namespace autoware::signal_processing +{ /** * @class First-order low-pass filter * @brief filtering values @@ -51,5 +53,6 @@ class LowpassFilterTwist : public LowpassFilterInterface -namespace signal_processing +namespace autoware::signal_processing { double lowpassFilter(const double current_val, const double prev_val, const double gain); -} /** * @class First-order low-pass filter @@ -43,5 +42,6 @@ class LowpassFilter1d boost::optional getValue() const; double filter(const double u); }; +} // namespace autoware::signal_processing -#endif // SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ +#endif // AUTOWARE__SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ diff --git a/common/signal_processing/package.xml b/common/autoware_signal_processing/package.xml similarity index 96% rename from common/signal_processing/package.xml rename to common/autoware_signal_processing/package.xml index 2412f916dbd15..18841347dab9f 100644 --- a/common/signal_processing/package.xml +++ b/common/autoware_signal_processing/package.xml @@ -1,7 +1,7 @@ - signal_processing + autoware_signal_processing 0.1.0 The signal processing package Takayuki Murooka diff --git a/common/signal_processing/src/butterworth.cpp b/common/autoware_signal_processing/src/butterworth.cpp similarity index 98% rename from common/signal_processing/src/butterworth.cpp rename to common/autoware_signal_processing/src/butterworth.cpp index a0e4d61f1412b..7f491d443787b 100644 --- a/common/signal_processing/src/butterworth.cpp +++ b/common/autoware_signal_processing/src/butterworth.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/butterworth.hpp" +#include "autoware/signal_processing/butterworth.hpp" #include @@ -20,6 +20,8 @@ #include #include +namespace autoware::signal_processing +{ /** * @brief Computes the minimum of an analog Butterworth filter order and cut-off frequency give * the pass and stop-band frequencies and ripple magnitude (tolerances). @@ -341,3 +343,4 @@ void ButterworthFilter::printFilterSpecs() const rclcpp::get_logger("rclcpp"), "Cut-off Frequency : %2.2f rad/sec", this->filter_specs_.Wc_rad_sec); } +} // namespace autoware::signal_processing diff --git a/common/signal_processing/src/lowpass_filter.cpp b/common/autoware_signal_processing/src/lowpass_filter.cpp similarity index 89% rename from common/signal_processing/src/lowpass_filter.cpp rename to common/autoware_signal_processing/src/lowpass_filter.cpp index c5119828f4c52..6d275bb5023b3 100644 --- a/common/signal_processing/src/lowpass_filter.cpp +++ b/common/autoware_signal_processing/src/lowpass_filter.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter.hpp" +#include "autoware/signal_processing/lowpass_filter.hpp" +namespace autoware::signal_processing +{ geometry_msgs::msg::Twist LowpassFilterTwist::filter(const geometry_msgs::msg::Twist & u) { if (x_) { @@ -31,3 +33,4 @@ geometry_msgs::msg::Twist LowpassFilterTwist::filter(const geometry_msgs::msg::T x_ = u; return x_.get(); } +} // namespace autoware::signal_processing diff --git a/common/signal_processing/src/lowpass_filter_1d.cpp b/common/autoware_signal_processing/src/lowpass_filter_1d.cpp similarity index 89% rename from common/signal_processing/src/lowpass_filter_1d.cpp rename to common/autoware_signal_processing/src/lowpass_filter_1d.cpp index 08de005f736c1..f5835ae160ba7 100644 --- a/common/signal_processing/src/lowpass_filter_1d.cpp +++ b/common/autoware_signal_processing/src/lowpass_filter_1d.cpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" -namespace signal_processing +namespace autoware::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) { @@ -57,3 +56,4 @@ double LowpassFilter1d::filter(const double u) x_ = u; return x_.get(); } +} // namespace autoware::signal_processing diff --git a/common/signal_processing/test/include/butterworth_filter_test.hpp b/common/autoware_signal_processing/test/include/butterworth_filter_test.hpp similarity index 94% rename from common/signal_processing/test/include/butterworth_filter_test.hpp rename to common/autoware_signal_processing/test/include/butterworth_filter_test.hpp index 6a43b0b95a7e2..83b060373a442 100644 --- a/common/signal_processing/test/include/butterworth_filter_test.hpp +++ b/common/autoware_signal_processing/test/include/butterworth_filter_test.hpp @@ -15,9 +15,9 @@ #ifndef BUTTERWORTH_FILTER_TEST_HPP_ #define BUTTERWORTH_FILTER_TEST_HPP_ +#include "autoware/signal_processing/butterworth.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "signal_processing/butterworth.hpp" class ButterWorthTestFixture : public ::testing::Test { diff --git a/common/signal_processing/test/src/butterworth_filter_test.cpp b/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp similarity index 98% rename from common/signal_processing/test/src/butterworth_filter_test.cpp rename to common/autoware_signal_processing/test/src/butterworth_filter_test.cpp index d85b9a37f870f..bd5d1dbbf6948 100644 --- a/common/signal_processing/test/src/butterworth_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp @@ -14,6 +14,8 @@ #include "butterworth_filter_test.hpp" +using autoware::signal_processing::ButterworthFilter; + TEST_F(ButterWorthTestFixture, butterworthOrderTest) { double tol = 1e-4; diff --git a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp similarity index 92% rename from common/signal_processing/test/src/lowpass_filter_1d_test.cpp rename to common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp index 5cce36884588e..44e8a1ffce437 100644 --- a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include constexpr double epsilon = 1e-6; +using autoware::signal_processing::LowpassFilter1d; + TEST(lowpass_filter_1d, filter) { LowpassFilter1d lowpass_filter_1d(0.1); diff --git a/common/signal_processing/test/src/lowpass_filter_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp similarity index 95% rename from common/signal_processing/test/src/lowpass_filter_test.cpp rename to common/autoware_signal_processing/test/src/lowpass_filter_test.cpp index 8dfea4dcae02e..864378719dd5c 100644 --- a/common/signal_processing/test/src/lowpass_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter.hpp" +#include "autoware/signal_processing/lowpass_filter.hpp" #include constexpr double epsilon = 1e-6; +using autoware::signal_processing::LowpassFilterTwist; + geometry_msgs::msg::Twist createTwist( const double lx, const double ly, const double lz, const double ax, const double ay, const double az) diff --git a/common/signal_processing/usage_examples/main_butterworth.cpp b/common/autoware_signal_processing/usage_examples/main_butterworth.cpp similarity index 98% rename from common/signal_processing/usage_examples/main_butterworth.cpp rename to common/autoware_signal_processing/usage_examples/main_butterworth.cpp index 2b36bd40b3578..5de87af046879 100644 --- a/common/signal_processing/usage_examples/main_butterworth.cpp +++ b/common/autoware_signal_processing/usage_examples/main_butterworth.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/butterworth.hpp" +#include "autoware/signal_processing/butterworth.hpp" #include #include diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp index 38a498f69bbae..f9ee6e1beeefa 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp @@ -17,6 +17,8 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include +#include #include #include @@ -33,6 +35,10 @@ class Vector2d Vector2d(const double x, const double y) : x_(x), y_(y) {} + explicit Vector2d(const autoware::universe_utils::Point2d & point) : x_(point.x()), y_(point.y()) + { + } + double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); } double dot(const Vector2d & other) const { return x_ * other.x() + y_ * other.y(); } @@ -60,38 +66,6 @@ class Vector2d double y_; }; -// We use Vector2d to represent points, but we do not name the class Point2d directly -// as it has some vector operation functions. -using Point2d = Vector2d; -using Points2d = std::vector; - -class ConvexPolygon2d -{ -public: - explicit ConvexPolygon2d(const Points2d & vertices) - { - if (vertices.size() < 3) { - throw std::invalid_argument("At least 3 points are required for vertices."); - } - vertices_ = vertices; - } - - explicit ConvexPolygon2d(Points2d && vertices) - { - if (vertices.size() < 3) { - throw std::invalid_argument("At least 3 points are required for vertices."); - } - vertices_ = std::move(vertices); - } - - const Points2d & vertices() const { return vertices_; } - - Points2d & vertices() { return vertices_; } - -private: - Points2d vertices_; -}; - inline Vector2d operator+(const Vector2d & v1, const Vector2d & v2) { return {v1.x() + v2.x(), v1.y() + v2.y()}; @@ -112,20 +86,76 @@ inline Vector2d operator*(const double & s, const Vector2d & v) return {s * v.x(), s * v.y()}; } -Point2d from_boost(const autoware::universe_utils::Point2d & point); +// We use Vector2d to represent points, but we do not name the class Point2d directly +// as it has some vector operation functions. +using Point2d = Vector2d; +using Points2d = std::vector; +using PointList2d = std::list; + +class Polygon2d +{ +public: + static std::optional create( + const PointList2d & outer, const std::vector & inners) noexcept; + + static std::optional create( + PointList2d && outer, std::vector && inners) noexcept; + + static std::optional create( + const autoware::universe_utils::Polygon2d & polygon) noexcept; + + const PointList2d & outer() const noexcept { return outer_; } + + PointList2d & outer() noexcept { return outer_; } + + const std::vector & inners() const noexcept { return inners_; } + + std::vector & inners() noexcept { return inners_; } + + autoware::universe_utils::Polygon2d to_boost() const; + +protected: + Polygon2d(const PointList2d & outer, const std::vector & inners) + : outer_(outer), inners_(inners) + { + } + + Polygon2d(PointList2d && outer, std::vector && inners) + : outer_(std::move(outer)), inners_(std::move(inners)) + { + } + + PointList2d outer_; -ConvexPolygon2d from_boost(const autoware::universe_utils::Polygon2d & polygon); + std::vector inners_; +}; + +class ConvexPolygon2d : public Polygon2d +{ +public: + static std::optional create(const PointList2d & vertices) noexcept; + + static std::optional create(PointList2d && vertices) noexcept; + + static std::optional create( + const autoware::universe_utils::Polygon2d & polygon) noexcept; + + const PointList2d & vertices() const noexcept { return outer(); } -autoware::universe_utils::Point2d to_boost(const Point2d & point); + PointList2d & vertices() noexcept { return outer(); } -autoware::universe_utils::Polygon2d to_boost(const ConvexPolygon2d & polygon); +private: + explicit ConvexPolygon2d(const PointList2d & vertices) : Polygon2d(vertices, {}) {} + + explicit ConvexPolygon2d(PointList2d && vertices) : Polygon2d(std::move(vertices), {}) {} +}; } // namespace alt double area(const alt::ConvexPolygon2d & poly); -alt::ConvexPolygon2d convex_hull(const alt::Points2d & points); +std::optional convex_hull(const alt::Points2d & points); -void correct(alt::ConvexPolygon2d & poly); +void correct(alt::Polygon2d & poly); bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly); @@ -136,9 +166,14 @@ double distance( double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly); +std::optional envelope(const alt::Polygon2d & poly); + bool equals(const alt::Point2d & point1, const alt::Point2d & point2); -bool equals(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2); +bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2); + +alt::Points2d::const_iterator find_farthest( + const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end); bool intersects( const alt::Point2d & seg1_start, const alt::Point2d & seg1_end, const alt::Point2d & seg2_start, @@ -149,7 +184,11 @@ bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & bool is_above( const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end); -bool is_clockwise(const alt::ConvexPolygon2d & poly); +bool is_clockwise(const alt::PointList2d & vertices); + +bool is_convex(const alt::Polygon2d & poly); + +alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance); bool touches( const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end); diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp index 5ba6501a1ec18..e111504795861 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp @@ -15,93 +15,92 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ #define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/alt_geometry.hpp" -#include +#include #include namespace autoware::universe_utils { - -class EarClipping +struct LinkedPoint { -public: - std::vector indices; - std::size_t vertices = 0; - using Polygon2d = autoware::universe_utils::Polygon2d; - using Point2d = autoware::universe_utils::Point2d; - using LinearRing2d = autoware::universe_utils::LinearRing2d; - - void operator()(const Polygon2d & polygon); - - ~EarClipping() + explicit LinkedPoint(const alt::Point2d & point) + : pt(point), steiner(false), prev_index(std::nullopt), next_index(std::nullopt) { - for (auto * p : points_) { - delete p; - } } -private: - struct Point - { - Point(const std::size_t index, Point2d point) : i(index), pt(std::move(point)) {} - - const std::size_t i; // Index of the point in the original polygon - const Point2d pt; // The Point2d object representing the coordinates - - // Previous and next vertices (Points) in the polygon ring - Point * prev = nullptr; - Point * next = nullptr; - bool steiner = false; - - [[nodiscard]] double x() const { return pt.x(); } - [[nodiscard]] double y() const { return pt.y(); } - }; - - std::vector points_; - - Point * linked_list(const LinearRing2d & points, bool clockwise); - static Point * filter_points(Point * start, Point * end = nullptr); - Point * cure_local_intersections(Point * start); - static Point * get_leftmost(Point * start); - Point * split_polygon(Point * a, Point * b); - Point * insert_point(std::size_t i, const Point2d & p, Point * last); - Point * eliminate_holes( - const std::vector & inners, EarClipping::Point * outer_point); - Point * eliminate_hole(Point * hole, Point * outer_point); - static Point * find_hole_bridge(Point * hole, Point * outer_point); - void ear_clipping_linked(Point * ear, int pass = 0); - void split_ear_clipping(Point * start); - static void remove_point(Point * p); - static bool is_ear(Point * ear); - static bool sector_contains_sector(const Point * m, const Point * p); - [[nodiscard]] static bool point_in_triangle( - double ax, double ay, double bx, double by, double cx, double cy, double px, double py); - static bool is_valid_diagonal(Point * a, Point * b); - static bool equals(const Point * p1, const Point * p2); - static bool intersects(const Point * p1, const Point * q1, const Point * p2, const Point * q2); - static bool on_segment(const Point * p, const Point * q, const Point * r); - static bool intersects_polygon(const Point * a, const Point * b); - static bool locally_inside(const Point * a, const Point * b); - static bool middle_inside(const Point * a, const Point * b); - static int sign(double val); - static double area(const Point * p, const Point * q, const Point * r); - - // Function to construct a new Point object - EarClipping::Point * construct_point(std::size_t index, const Point2d & point) - { - auto * new_point = new Point(index, point); - points_.push_back(new_point); - return new_point; - } + alt::Point2d pt; + bool steiner; + std::optional prev_index; + std::optional next_index; + [[nodiscard]] double x() const { return pt.x(); } + [[nodiscard]] double y() const { return pt.y(); } }; -/// @brief Triangulate based on ear clipping algorithm -/// @param polygon concave/convex polygon with/without holes -/// @details algorithm based on https://github.com/mapbox/earclipping with modification -std::vector triangulate( - const autoware::universe_utils::Polygon2d & poly); - +/** + * @brief main ear slicing loop which triangulates a polygon using linked list + * @details iterates over the linked list of polygon points, cutting off triangular ears one by one + * handles different stages for fixing intersections and splitting if necessary + */ +void ear_clipping_linked( + std::size_t ear_index, std::vector & indices, std::vector & points, + const int pass = 0); +void split_ear_clipping( + std::vector & points, std::size_t start_index, std::vector & indices); + +/** + * @brief creates a linked list from a ring of points + * @details converts a polygon ring into a doubly linked list with optional clockwise ordering + * @return the last index of the created linked list + */ +std::size_t linked_list( + const alt::PointList2d & ring, const bool clockwise, std::size_t & vertices, + std::vector & points); + +/** + * @brief David Eberly's algorithm for finding a bridge between hole and outer polygon + * @details connects a hole to the outer polygon by finding the closest bridge point + * @return index of the bridge point + */ +std::size_t find_hole_bridge( + const std::size_t hole_index, const std::size_t outer_point_index, + const std::vector & points); + +/** + * @brief eliminates a single hole by connecting it to the outer polygon + * @details finds a bridge and modifies the linked list to remove the hole + * @return the updated outer_index after the hole is eliminated + */ +std::size_t eliminate_hole( + const std::size_t hole_index, const std::size_t outer_index, std::vector & points); + +/** + * @brief eliminates all holes from a polygon + * @details processes multiple holes by connecting each to the outer polygon in sequence + * @return the updated outer_index after all holes are eliminated + */ +std::size_t eliminate_holes( + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + std::vector & points); + +/** + * @brief triangulates a polygon into convex triangles + * @details simplifies a concave polygon, with or without holes, into a set of triangles + * the size of the `points` vector at the end of the perform_triangulation algorithm is described as + * follow: + * - `outer_points`: Number of points in the initial outer linked list. + * - `hole_points`: Number of points in all inner polygons. + * - `2 * n_holes`: Additional points for bridging holes, where `n_holes` is the number of holes. + * the final size of `points` vector is: `outer_points + hole_points + 2 * n_holes`. + * @return A vector of convex triangles representing the triangulated polygon. + */ +std::vector triangulate(const alt::Polygon2d & polygon); + +/** + * @brief Boost.Geometry version of triangulate() + * @return A vector of convex triangles representing the triangulated polygon. + */ +std::vector triangulate(const Polygon2d & polygon); } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 3271f6d299d3d..6c060b03b4543 100644 --- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp @@ -19,35 +19,124 @@ namespace autoware::universe_utils // Alternatives for Boost.Geometry ---------------------------------------------------------------- namespace alt { -Point2d from_boost(const autoware::universe_utils::Point2d & point) +std::optional Polygon2d::create( + const PointList2d & outer, const std::vector & inners) noexcept { - return {point.x(), point.y()}; + Polygon2d poly(outer, inners); + correct(poly); + + if (poly.outer().size() < 4) { + return std::nullopt; + } + + for (const auto & inner : poly.inners()) { + if (inner.size() < 4) { + return std::nullopt; + } + } + + return poly; } -ConvexPolygon2d from_boost(const autoware::universe_utils::Polygon2d & polygon) +std::optional Polygon2d::create( + PointList2d && outer, std::vector && inners) noexcept { - Points2d points; + Polygon2d poly(std::move(outer), std::move(inners)); + correct(poly); + + if (poly.outer().size() < 4) { + return std::nullopt; + } + + for (const auto & inner : poly.inners()) { + if (inner.size() < 4) { + return std::nullopt; + } + } + + return poly; +} + +std::optional Polygon2d::create( + const autoware::universe_utils::Polygon2d & polygon) noexcept +{ + PointList2d outer; for (const auto & point : polygon.outer()) { - points.push_back(from_boost(point)); + outer.push_back(Point2d(point)); + } + + std::vector inners; + for (const auto & inner : polygon.inners()) { + PointList2d _inner; + for (const auto & point : inner) { + _inner.push_back(Point2d(point)); + } + inners.push_back(_inner); + } + + return Polygon2d::create(outer, inners); +} + +autoware::universe_utils::Polygon2d Polygon2d::to_boost() const +{ + autoware::universe_utils::Polygon2d polygon; + + for (const auto & point : outer_) { + polygon.outer().emplace_back(point.x(), point.y()); + } + + for (const auto & inner : inners_) { + autoware::universe_utils::LinearRing2d _inner; + for (const auto & point : inner) { + _inner.emplace_back(point.x(), point.y()); + } + polygon.inners().push_back(_inner); } - ConvexPolygon2d _polygon(points); - correct(_polygon); - return _polygon; + return polygon; } -autoware::universe_utils::Point2d to_boost(const Point2d & point) +std::optional ConvexPolygon2d::create(const PointList2d & vertices) noexcept { - return {point.x(), point.y()}; + ConvexPolygon2d poly(vertices); + correct(poly); + + if (poly.vertices().size() < 4) { + return std::nullopt; + } + + if (!is_convex(poly)) { + return std::nullopt; + } + + return poly; } -autoware::universe_utils::Polygon2d to_boost(const ConvexPolygon2d & polygon) +std::optional ConvexPolygon2d::create(PointList2d && vertices) noexcept { - autoware::universe_utils::Polygon2d _polygon; - for (const auto & vertex : polygon.vertices()) { - _polygon.outer().push_back(to_boost(vertex)); + ConvexPolygon2d poly(std::move(vertices)); + correct(poly); + + if (poly.vertices().size() < 4) { + return std::nullopt; + } + + if (!is_convex(poly)) { + return std::nullopt; } - return _polygon; + + return poly; +} + +std::optional ConvexPolygon2d::create( + const autoware::universe_utils::Polygon2d & polygon) noexcept +{ + PointList2d vertices; + for (const auto & point : polygon.outer()) { + vertices.push_back(Point2d(point)); + } + + return ConvexPolygon2d::create(vertices); } } // namespace alt @@ -56,17 +145,17 @@ double area(const alt::ConvexPolygon2d & poly) const auto & vertices = poly.vertices(); double area = 0.; - for (size_t i = 1; i < vertices.size() - 1; ++i) { - area += (vertices[i + 1] - vertices.front()).cross(vertices[i] - vertices.front()) / 2; + for (auto it = std::next(vertices.cbegin()); it != std::prev(vertices.cend(), 2); ++it) { + area += (*std::next(it) - vertices.front()).cross(*it - vertices.front()) / 2; } return area; } -alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) +std::optional convex_hull(const alt::Points2d & points) { if (points.size() < 3) { - throw std::invalid_argument("At least 3 points are required for calculating convex hull."); + return std::nullopt; } // QuickHull algorithm @@ -76,7 +165,7 @@ alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) const auto & p_min = *p_minmax_itr.first; const auto & p_max = *p_minmax_itr.second; - alt::Points2d vertices; + alt::PointList2d vertices; if (points.size() == 3) { std::rotate_copy( @@ -89,11 +178,7 @@ alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) return; } - const auto farthest = - *std::max_element(points.begin(), points.end(), [&](const auto & a, const auto & b) { - const auto seg_vec = p2 - p1; - return seg_vec.cross(a - p1) < seg_vec.cross(b - p1); - }); + const auto & farthest = *find_farthest(points, p1, p2); alt::Points2d subset1, subset2; for (const auto & point : points) { @@ -124,23 +209,35 @@ alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) make_hull(make_hull, p_max, p_min, below_points); } - alt::ConvexPolygon2d hull(vertices); - correct(hull); + auto hull = alt::ConvexPolygon2d::create(vertices); + if (!hull) { + return std::nullopt; + } return hull; } -void correct(alt::ConvexPolygon2d & poly) +void correct(alt::Polygon2d & poly) { - auto & vertices = poly.vertices(); + auto correct_vertices = [](alt::PointList2d & vertices) { + // remove adjacent duplicate points + const auto it = std::unique( + vertices.begin(), vertices.end(), + [](const auto & a, const auto & b) { return equals(a, b); }); + vertices.erase(it, vertices.end()); + + if (!equals(vertices.front(), vertices.back())) { + vertices.push_back(vertices.front()); + } - // sort points in clockwise order with respect to the first point - std::sort(vertices.begin() + 1, vertices.end(), [&](const auto & a, const auto & b) { - return (a - vertices.front()).cross(b - vertices.front()) < 0; - }); + if (!is_clockwise(vertices)) { + std::reverse(std::next(vertices.begin()), std::prev(vertices.end())); + } + }; - if (equals(vertices.front(), vertices.back())) { - vertices.pop_back(); + correct_vertices(poly.outer()); + for (auto & inner : poly.inners()) { + correct_vertices(inner); } } @@ -149,19 +246,19 @@ bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) constexpr double epsilon = 1e-6; const auto & vertices = poly.vertices(); - const auto num_of_vertices = vertices.size(); - int64_t winding_number = 0; + std::size_t winding_number = 0; const auto [y_min_vertex, y_max_vertex] = std::minmax_element( - vertices.begin(), vertices.end(), [](const auto & a, const auto & b) { return a.y() < b.y(); }); + vertices.begin(), std::prev(vertices.end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); if (point.y() < y_min_vertex->y() || point.y() > y_max_vertex->y()) { return false; } double cross; - for (size_t i = 0; i < num_of_vertices; ++i) { - const auto & p1 = vertices[i]; - const auto & p2 = vertices[(i + 1) % num_of_vertices]; + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + const auto & p1 = *it; + const auto & p2 = *std::next(it); if (p1.y() <= point.y() && p2.y() >= point.y()) { // upward edge cross = (p2 - p1).cross(point - p1); @@ -235,26 +332,61 @@ double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) // TODO(mitukou1109): Use plane sweep method to improve performance? const auto & vertices = poly.vertices(); double min_distance = std::numeric_limits::max(); - for (size_t i = 0; i < vertices.size(); ++i) { - min_distance = - std::min(min_distance, distance(point, vertices[i], vertices[(i + 1) % vertices.size()])); + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + min_distance = std::min(min_distance, distance(point, *it, *std::next(it))); } return min_distance; } +std::optional envelope(const alt::Polygon2d & poly) +{ + const auto [x_min_vertex, x_max_vertex] = std::minmax_element( + poly.outer().begin(), std::prev(poly.outer().end()), + [](const auto & a, const auto & b) { return a.x() < b.x(); }); + + const auto [y_min_vertex, y_max_vertex] = std::minmax_element( + poly.outer().begin(), std::prev(poly.outer().end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); + + return alt::ConvexPolygon2d::create(alt::PointList2d{ + {x_min_vertex->x(), y_min_vertex->y()}, + {x_min_vertex->x(), y_max_vertex->y()}, + {x_max_vertex->x(), y_max_vertex->y()}, + {x_max_vertex->x(), y_min_vertex->y()}}); +} + bool equals(const alt::Point2d & point1, const alt::Point2d & point2) { constexpr double epsilon = 1e-3; return std::abs(point1.x() - point2.x()) < epsilon && std::abs(point1.y() - point2.y()) < epsilon; } -bool equals(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2) +bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2) +{ + const auto outer_equals = std::equal( + poly1.outer().begin(), std::prev(poly1.outer().end()), poly2.outer().begin(), + std::prev(poly2.outer().end()), [](const auto & a, const auto & b) { return equals(a, b); }); + + auto inners_equal = true; + for (const auto & inner1 : poly1.inners()) { + inners_equal &= + std::any_of(poly2.inners().begin(), poly2.inners().end(), [&](const auto & inner2) { + return std::equal( + inner1.begin(), std::prev(inner1.end()), inner2.begin(), std::prev(inner2.end()), + [](const auto & a, const auto & b) { return equals(a, b); }); + }); + } + + return outer_equals && inners_equal; +} + +alt::Points2d::const_iterator find_farthest( + const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end) { - return std::all_of(poly1.vertices().begin(), poly1.vertices().end(), [&](const auto & a) { - return std::any_of(poly2.vertices().begin(), poly2.vertices().end(), [&](const auto & b) { - return equals(a, b); - }); + const auto seg_vec = seg_end - seg_start; + return std::max_element(points.begin(), points.end(), [&](const auto & a, const auto & b) { + return std::abs(seg_vec.cross(a - seg_start)) < std::abs(seg_vec.cross(b - seg_start)); }); } @@ -297,7 +429,7 @@ bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & auto find_farthest_vertex = [](const alt::ConvexPolygon2d & poly, const alt::Vector2d & direction) { return std::max_element( - poly.vertices().begin(), poly.vertices().end(), + poly.vertices().begin(), std::prev(poly.vertices().end()), [&](const auto & a, const auto & b) { return direction.dot(a) <= direction.dot(b); }); }; return *find_farthest_vertex(poly1, direction) - *find_farthest_vertex(poly2, -direction); @@ -342,9 +474,75 @@ bool is_above( return (seg_end - seg_start).cross(point - seg_start) > 0; } -bool is_clockwise(const alt::ConvexPolygon2d & poly) +bool is_clockwise(const alt::PointList2d & vertices) +{ + double sum = 0.; + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + sum += (std::next(it)->x() - it->x()) * (std::next(it)->y() + it->y()); + } + + return sum > 0; +} + +bool is_convex(const alt::Polygon2d & poly) { - return area(poly) > 0; + constexpr double epsilon = 1e-6; + + if (!poly.inners().empty()) { + return false; + } + + const auto & outer = poly.outer(); + + for (auto it = std::next(outer.cbegin()); it != std::prev(outer.cend()); ++it) { + const auto & p1 = *--it; + const auto & p2 = *it; + const auto & p3 = *++it; + + if ((p2 - p1).cross(p3 - p2) > epsilon) { + return false; + } + } + + return true; +} + +alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance) +{ + if (line.size() < 3) { + return line; + } + + alt::Points2d pending(std::next(line.begin()), std::prev(line.end())); + alt::PointList2d simplified; + + // Douglas-Peucker algorithm + + auto douglas_peucker = [&max_distance, &pending, &simplified]( + auto self, const alt::Point2d & seg_start, + const alt::Point2d & seg_end) { + if (pending.empty()) { + return; + } + + const auto farthest_itr = find_farthest(pending, seg_start, seg_end); + const auto farthest = *farthest_itr; + pending.erase(farthest_itr); + + if (distance(farthest, seg_start, seg_end) <= max_distance) { + return; + } + + self(self, seg_start, farthest); + simplified.push_back(farthest); + self(self, farthest, seg_end); + }; + + simplified.push_back(line.front()); + douglas_peucker(douglas_peucker, line.front(), line.back()); + simplified.push_back(line.back()); + + return simplified; } bool touches( @@ -362,17 +560,17 @@ bool touches( bool touches(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) { const auto & vertices = poly.vertices(); - const auto num_of_vertices = vertices.size(); const auto [y_min_vertex, y_max_vertex] = std::minmax_element( - vertices.begin(), vertices.end(), [](const auto & a, const auto & b) { return a.y() < b.y(); }); + vertices.begin(), std::prev(vertices.end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); if (point.y() < y_min_vertex->y() || point.y() > y_max_vertex->y()) { return false; } - for (size_t i = 0; i < num_of_vertices; ++i) { + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { // check if the point is on each edge of the polygon - if (touches(point, vertices[i], vertices[(i + 1) % num_of_vertices])) { + if (touches(point, *it, *std::next(it))) { return true; } } @@ -385,19 +583,19 @@ bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) constexpr double epsilon = 1e-6; const auto & vertices = poly.vertices(); - const auto num_of_vertices = vertices.size(); int64_t winding_number = 0; const auto [y_min_vertex, y_max_vertex] = std::minmax_element( - vertices.begin(), vertices.end(), [](const auto & a, const auto & b) { return a.y() < b.y(); }); + vertices.begin(), std::prev(vertices.end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); if (point.y() <= y_min_vertex->y() || point.y() >= y_max_vertex->y()) { return false; } double cross; - for (size_t i = 0; i < num_of_vertices; ++i) { - const auto & p1 = vertices[i]; - const auto & p2 = vertices[(i + 1) % num_of_vertices]; + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + const auto & p1 = *it; + const auto & p2 = *std::next(it); if (p1.y() < point.y() && p2.y() > point.y()) { // upward edge cross = (p2 - p1).cross(point - p1); diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 741f6fb901c07..595c93d073ee3 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -14,525 +14,618 @@ #include "autoware/universe_utils/geometry/ear_clipping.hpp" -#ifndef EAR_CUT_IMPL_HPP -#define EAR_CUT_IMPL_HPP - namespace autoware::universe_utils { -void EarClipping::operator()(const Polygon2d & polygon) +void remove_point(const std::size_t p_index, std::vector & points) { - indices.clear(); - vertices = 0; - - if (polygon.outer().size() == 0) return; - - std::size_t len = 0; - - const auto & outer_ring = polygon.outer(); - len = outer_ring.size(); - points_.reserve(len * 3 / 2); - indices.reserve(len + outer_ring.size()); + std::size_t prev_index = points[p_index].prev_index.value(); + std::size_t next_index = points[p_index].next_index.value(); - EarClipping::Point * outer_point = linked_list(outer_ring, true); - if (!outer_point || outer_point->prev == outer_point->next) return; - if (polygon.inners().size() > 0) outer_point = eliminate_holes(polygon.inners(), outer_point); - ear_clipping_linked(outer_point); - points_.clear(); + points[prev_index].next_index = next_index; + points[next_index].prev_index = prev_index; } -/// @brief create a circular doubly linked list from polygon points in the specified winding order -EarClipping::Point * EarClipping::linked_list(const LinearRing2d & points, bool clockwise) +std::size_t get_leftmost(const std::size_t start_idx, const std::vector & points) { - double sum = 0; - const std::size_t len = points.size(); - EarClipping::Point * last = nullptr; - - for (size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { - const auto & p1 = points[i]; - const auto & p2 = points[j]; - const double p10 = p1.x(); - const double p11 = p1.y(); - const double p20 = p2.x(); - const double p21 = p2.y(); - sum += (p20 - p10) * (p11 + p21); - } + std::optional p_idx = points[start_idx].next_index; + std::size_t left_most_idx = start_idx; - if (clockwise == (sum > 0)) { - for (size_t i = 0; i < len; i++) { - last = insert_point(vertices + i, points[i], last); - } - } else { - for (size_t i = len; i-- > 0;) { - last = insert_point(vertices + i, points[i], last); + while (p_idx.has_value() && p_idx.value() != start_idx) { + if ( + points[p_idx.value()].x() < points[left_most_idx].x() || + (points[p_idx.value()].x() == points[left_most_idx].x() && + points[p_idx.value()].y() < points[left_most_idx].y())) { + left_most_idx = p_idx.value(); } + p_idx = points[p_idx.value()].next_index; } - if (last && equals(last, last->next)) { - remove_point(last); - last = last->next; - } - - vertices += len; + return left_most_idx; +} - return last; +bool point_in_triangle( + const double ax, const double ay, const double bx, const double by, const double cx, + const double cy, const double px, const double py) +{ + return (cx - px) * (ay - py) >= (ax - px) * (cy - py) && + (ax - px) * (by - py) >= (bx - px) * (ay - py) && + (bx - px) * (cy - py) >= (cx - px) * (by - py); } -/// @brief eliminate colinear or duplicate points -EarClipping::Point * EarClipping::filter_points( - EarClipping::Point * start, EarClipping::Point * end) +double area( + const std::vector & points, const std::size_t p_idx, const std::size_t q_idx, + const std::size_t r_idx) { - if (!end) end = start; + const LinkedPoint & p = points[p_idx]; + const LinkedPoint & q = points[q_idx]; + const LinkedPoint & r = points[r_idx]; + return (q.y() - p.y()) * (r.x() - q.x()) - (q.x() - p.x()) * (r.y() - q.y()); +} - EarClipping::Point * p = start; - bool again = false; - do { - again = false; +bool middle_inside( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) +{ + std::optional p_idx = a_idx; + bool inside = false; + double px = (points[a_idx].x() + points[b_idx].x()) / 2; + double py = (points[a_idx].y() + points[b_idx].y()) / 2; + std::size_t start_idx = a_idx; - if (!p->steiner && (equals(p, p->next) || area(p->prev, p, p->next) == 0)) { - remove_point(p); - p = end = p->prev; + while (p_idx.has_value()) { + std::size_t current_idx = p_idx.value(); + std::size_t next_idx = points[current_idx].next_index.value(); - if (p == p->next) break; - again = true; + if ( + ((points[current_idx].y() > py) != (points[next_idx].y() > py)) && + points[next_idx].y() != points[current_idx].y() && + (px < (points[next_idx].x() - points[current_idx].x()) * (py - points[current_idx].y()) / + (points[next_idx].y() - points[current_idx].y()) + + points[current_idx].x())) { + inside = !inside; + } - } else { - p = p->next; + if (next_idx == start_idx) { + break; // Break the loop if we have cycled back to the start index } - } while (again || p != end); - return end; + p_idx = next_idx; + } + + return inside; } -/// @brief find a bridge between vertices that connects hole with an outer ring and and link it -EarClipping::Point * EarClipping::eliminate_hole( - EarClipping::Point * hole, EarClipping::Point * outer_point) +bool equals( + const std::size_t p1_idx, const std::size_t p2_idx, const std::vector & points) { - EarClipping::Point * bridge = find_hole_bridge(hole, outer_point); - if (!bridge) { - return outer_point; - } - EarClipping::Point * bridge_reverse = split_polygon(bridge, hole); + return points[p1_idx].x() == points[p2_idx].x() && points[p1_idx].y() == points[p2_idx].y(); +} - // filter collinear points around the cuts - filter_points(bridge_reverse, bridge_reverse->next); +int sign(const double val) +{ + return (0.0 < val) - (val < 0.0); +} - // Check if input node was removed by the filtering - return filter_points(bridge, bridge->next); +bool on_segment( + const std::vector & points, const std::size_t p_idx, const std::size_t q_idx, + const std::size_t r_idx) +{ + const LinkedPoint & p = points[p_idx]; + const LinkedPoint & q = points[q_idx]; + const LinkedPoint & r = points[r_idx]; + return q.x() <= std::max(p.x(), r.x()) && q.x() >= std::min(p.x(), r.x()) && + q.y() <= std::max(p.y(), r.y()) && q.y() >= std::min(p.y(), r.y()); } -EarClipping::Point * EarClipping::eliminate_holes( - const std::vector & inners, EarClipping::Point * outer_point) +bool locally_inside( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) { - const size_t len = inners.size(); - - std::vector queue; - for (size_t i = 0; i < len; i++) { - Point * list = linked_list(inners[i], false); - if (list) { - if (list == list->next) list->steiner = true; - queue.push_back(get_leftmost(list)); - } - } - std::sort( - queue.begin(), queue.end(), [](const Point * a, const Point * b) { return a->x() < b->x(); }); - for (const auto & q : queue) { - outer_point = eliminate_hole(q, outer_point); + const auto & prev_idx = points[a_idx].prev_index; + const auto & next_idx = points[a_idx].next_index; + + if (!prev_idx.has_value() || !next_idx.has_value()) { + return false; } - return outer_point; + double area_prev = area(points, prev_idx.value(), a_idx, next_idx.value()); + double area_a_b_next = area(points, a_idx, b_idx, next_idx.value()); + double area_a_prev_b = area(points, a_idx, prev_idx.value(), b_idx); + double area_a_b_prev = area(points, a_idx, b_idx, prev_idx.value()); + double area_a_next_b = area(points, a_idx, next_idx.value(), b_idx); + + return area_prev < 0 ? area_a_b_next >= 0 && area_a_prev_b >= 0 + : area_a_b_prev < 0 || area_a_next_b < 0; } -// cspell: ignore Eberly -/// @brief David Eberly's algorithm for finding a bridge between hole and outer polygon -EarClipping::Point * EarClipping::find_hole_bridge(Point * hole, Point * outer_point) +bool intersects( + const std::size_t p1_idx, const std::size_t q1_idx, const std::size_t p2_idx, + const std::size_t q2_idx, const std::vector & points) { - Point * p = outer_point; - double hx = hole->x(); - double hy = hole->y(); - double qx = -std::numeric_limits::infinity(); - Point * m = nullptr; - do { - if (hy <= p->y() && hy >= p->next->y() && p->next->y() != p->y()) { - double x = p->x() + (hy - p->y()) * (p->next->x() - p->x()) / (p->next->y() - p->y()); - if (x <= hx && x > qx) { - qx = x; - m = p->x() < p->next->x() ? p : p->next; - if (x == hx) return m; - } - } - p = p->next; - } while (p != outer_point); + int o1 = sign(area(points, p1_idx, q1_idx, p2_idx)); + int o2 = sign(area(points, p1_idx, q1_idx, q2_idx)); + int o3 = sign(area(points, p2_idx, q2_idx, p1_idx)); + int o4 = sign(area(points, p2_idx, q2_idx, q1_idx)); - if (!m) return nullptr; + if (o1 != o2 && o3 != o4) return true; - const Point * stop = m; - double tan_min = std::numeric_limits::infinity(); + if (o1 == 0 && on_segment(points, p1_idx, p2_idx, q1_idx)) return true; + if (o2 == 0 && on_segment(points, p1_idx, q2_idx, q1_idx)) return true; + if (o3 == 0 && on_segment(points, p2_idx, p1_idx, q2_idx)) return true; + if (o4 == 0 && on_segment(points, p2_idx, q1_idx, q2_idx)) return true; - p = m; - double mx = m->x(); - double my = m->y(); + return false; +} - do { - if ( - hx >= p->x() && p->x() >= mx && hx != p->x() && - point_in_triangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p->x(), p->y())) { - const auto tan_cur = std::abs(hy - p->y()) / (hx - p->x()); +bool intersects_polygon( + const std::vector & points, const std::size_t a_idx, const std::size_t b_idx) +{ + std::size_t p_idx = a_idx; + std::optional p_next_opt = points[p_idx].next_index; - if ( - locally_inside(p, hole) && - (tan_cur < tan_min || - (tan_cur == tan_min && (p->x() > m->x() || sector_contains_sector(m, p))))) { - m = p; - tan_min = tan_cur; + while (p_next_opt.has_value() && p_next_opt.value() != a_idx) { + std::size_t p_next_idx = p_next_opt.value(); + + if (p_idx != a_idx && p_next_idx != a_idx && p_idx != b_idx && p_next_idx != b_idx) { + if (intersects(p_idx, p_next_idx, a_idx, b_idx, points)) { + return true; } } - p = p->next; - } while (p != stop); + p_idx = p_next_idx; + p_next_opt = points[p_idx].next_index; + } - return m; + return false; } -/// @brief main ear slicing loop which triangulates a polygon using linked list -void EarClipping::ear_clipping_linked(EarClipping::Point * ear, int pass) +bool is_valid_diagonal( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) { - if (!ear) return; + if ( + !points[a_idx].next_index.has_value() || !points[a_idx].prev_index.has_value() || + !points[b_idx].next_index.has_value() || !points[b_idx].prev_index.has_value()) { + return false; + } - EarClipping::Point * stop = ear; - EarClipping::Point * next = nullptr; + std::size_t a_next_idx = points[a_idx].next_index.value(); + std::size_t a_prev_idx = points[a_idx].prev_index.value(); + std::size_t b_next_idx = points[b_idx].next_index.value(); + std::size_t b_prev_idx = points[b_idx].prev_index.value(); - // Iterate through ears, slicing them one by one - while (ear->prev != ear->next) { - next = ear->next; + if (a_next_idx == b_idx || a_prev_idx == b_idx || intersects_polygon(points, a_idx, b_idx)) { + return false; + } - if (is_ear(ear)) { - // Cut off the triangle - indices.emplace_back(ear->prev->i); - indices.emplace_back(ear->i); - indices.emplace_back(next->i); + bool is_locally_inside_ab = locally_inside(a_idx, b_idx, points); + bool is_locally_inside_ba = locally_inside(b_idx, a_idx, points); + bool is_middle_inside = middle_inside(a_idx, b_idx, points); - remove_point(ear); - ear = next->next; - stop = next->next; + bool is_valid_diagonal = + (is_locally_inside_ab && is_locally_inside_ba && is_middle_inside && + (area(points, a_prev_idx, a_idx, b_prev_idx) != 0.0 || + area(points, a_idx, b_prev_idx, b_idx) != 0.0)) || + (equals(a_idx, b_idx, points) && area(points, a_prev_idx, a_idx, a_next_idx) > 0 && + area(points, b_prev_idx, b_idx, b_next_idx) > 0); - continue; - } + return is_valid_diagonal; +} - ear = next; - if (ear == stop) { - if (!pass) { - ear_clipping_linked(filter_points(ear), 1); - } else if (pass == 1) { - ear = cure_local_intersections(filter_points(ear)); - ear_clipping_linked(ear, 2); - } else if (pass == 2) { - split_ear_clipping(ear); - } - break; +std::size_t insert_point( + const alt::Point2d & pt, std::vector & points, + const std::optional last_index) +{ + std::size_t p_idx = points.size(); + points.push_back(LinkedPoint(pt)); + + // Making sure all next_index and prev_index will always have values + if (!last_index.has_value()) { + points[p_idx].prev_index = p_idx; + points[p_idx].next_index = p_idx; + } else { + std::size_t last = last_index.value(); + std::size_t next = points[last].next_index.value(); + points[p_idx].prev_index = last; + points[p_idx].next_index = next; + points[last].next_index = p_idx; + if (next != p_idx) { + points[next].prev_index = p_idx; } } + + return p_idx; } -/// @brief check whether ear is valid -bool EarClipping::is_ear(EarClipping::Point * ear) +std::size_t linked_list( + const alt::PointList2d & ring, const bool forward, std::size_t & vertices, + std::vector & points) { - const EarClipping::Point * a = ear->prev; - const EarClipping::Point * b = ear; - const EarClipping::Point * c = ear->next; + const std::size_t len = ring.size(); + std::optional last_index = std::nullopt; + + // create forward linked list if forward is true and ring is counter-clockwise, or + // forward is false and ring is clockwise + // create reverse linked list if forward is true and ring is clockwise, or + // forward is false and ring is counter-clockwise + if (forward == !is_clockwise(ring)) { + for (auto it = ring.begin(); it != ring.end(); ++it) { + last_index = insert_point(*it, points, last_index); + } + } else { + for (auto it = ring.rbegin(); it != ring.rend(); ++it) { + last_index = insert_point(*it, points, last_index); + } + } - if (area(a, b, c) >= 0) return false; // Reflex, can't be an ear + if (last_index.has_value()) { + std::size_t last_idx_value = last_index.value(); + std::optional next_index = points[last_idx_value].next_index; - EarClipping::Point * p = ear->next->next; + if (next_index.has_value() && equals(last_idx_value, next_index.value(), points)) { + std::size_t next_idx_value = next_index.value(); + remove_point(last_idx_value, points); + last_index = next_idx_value; + } + } - while (p != ear->prev) { - if ( - point_in_triangle(a->x(), a->y(), b->x(), b->y(), c->x(), c->y(), p->x(), p->y()) && - area(p->prev, p, p->next) >= 0) - return false; - p = p->next; + vertices += len; + return last_index.value(); +} + +bool sector_contains_sector( + const std::size_t m_idx, const std::size_t p_idx, const std::vector & points) +{ + if (!points[m_idx].prev_index.has_value() || !points[m_idx].next_index.has_value()) { + return false; } - return true; + std::size_t m_prev_idx = points[m_idx].prev_index.value(); + std::size_t m_next_idx = points[m_idx].next_index.value(); + + return area(points, m_prev_idx, m_idx, p_idx) < 0 && area(points, p_idx, m_next_idx, m_idx) < 0; } -/// @brief go through all polygon Points and cure small local self-intersections -EarClipping::Point * EarClipping::cure_local_intersections(EarClipping::Point * start) +std::size_t find_hole_bridge( + const std::size_t hole_index, const std::size_t outer_point_index, + const std::vector & points) { - EarClipping::Point * p = start; - do { - EarClipping::Point * a = p->prev; - EarClipping::Point * b = p->next->next; + std::size_t p = outer_point_index; + double hx = points[hole_index].x(); + double hy = points[hole_index].y(); + double qx = -std::numeric_limits::infinity(); + std::optional bridge_index = std::nullopt; + std::size_t next_index = points[p].next_index.value(); + while (p != outer_point_index) { if ( - !equals(a, b) && intersects(a, p, p->next, b) && locally_inside(a, b) && - locally_inside(b, a)) { - indices.emplace_back(a->i); - indices.emplace_back(p->i); - indices.emplace_back(b->i); - remove_point(p); - remove_point(p->next); - - p = start = b; + hy <= points[p].y() && hy >= points[next_index].y() && + points[next_index].y() != points[p].y()) { + double x = points[p].x() + (hy - points[p].y()) * (points[next_index].x() - points[p].x()) / + (points[next_index].y() - points[p].y()); + if (x <= hx && x > qx) { + qx = x; + bridge_index = (points[p].x() < points[next_index].x()) ? p : next_index; + if (x == hx) return bridge_index.value(); + } } - p = p->next; - } while (p != start); + p = next_index; + next_index = points[p].next_index.value(); + } - return filter_points(p); -} + if (!bridge_index.has_value()) return outer_point_index; -/// @brief splitting polygon into two and triangulate them independently -void EarClipping::split_ear_clipping(EarClipping::Point * start) -{ - EarClipping::Point * a = start; - do { - EarClipping::Point * b = a->next->next; - while (b != a->prev) { - if (a->i != b->i && is_valid_diagonal(a, b)) { - EarClipping::Point * c = split_polygon(a, b); + const std::size_t stop = bridge_index.value(); + double min_tan = std::numeric_limits::infinity(); - a = filter_points(a, a->next); - c = filter_points(c, c->next); + p = bridge_index.value(); + double mx = points[p].x(); + double my = points[p].y(); + next_index = points[p].next_index.value(); - ear_clipping_linked(a); - ear_clipping_linked(c); - return; + while (p != stop) { + if ( + hx >= points[p].x() && points[p].x() >= mx && hx != points[p].x() && + point_in_triangle( + hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, points[p].x(), points[p].y())) { + double current_tan = std::abs(hy - points[p].y()) / (hx - points[p].x()); + + if ( + locally_inside(p, hole_index, points) && + (current_tan < min_tan || + (current_tan == min_tan && (points[p].x() > points[bridge_index.value()].x() || + sector_contains_sector(bridge_index.value(), p, points))))) { + bridge_index = p; + min_tan = current_tan; } - b = b->next; } - a = a->next; - } while (a != start); -} -/// @brief check whether sector in vertex m contains sector in vertex p in the same coordinates -bool EarClipping::sector_contains_sector(const EarClipping::Point * m, const EarClipping::Point * p) -{ - return area(m->prev, m, p->prev) < 0 && area(p->next, m, m->next) < 0; + p = next_index; + next_index = points[p].next_index.value(); + } + + return bridge_index.value(); } -/// @brief find the leftmost Point of a polygon ring -EarClipping::Point * EarClipping::get_leftmost(EarClipping::Point * start) +std::size_t split_polygon( + std::size_t a_index, std::size_t b_index, std::vector & points) { - EarClipping::Point * p = start; - EarClipping::Point * leftmost = start; - do { - if (p->x() < leftmost->x() || (p->x() == leftmost->x() && p->y() < leftmost->y())) leftmost = p; - p = p->next; - } while (p != start); + std::size_t an_idx = points[a_index].next_index.value(); + std::size_t bp_idx = points[b_index].prev_index.value(); - return leftmost; -} + std::size_t a2_idx = points.size(); + std::size_t b2_idx = points.size() + 1; + points.push_back(points[a_index]); + points.push_back(points[b_index]); -/// @brief check if a point lies within a convex triangle -bool EarClipping::point_in_triangle( - double ax, double ay, double bx, double by, double cx, double cy, double px, double py) -{ - return (cx - px) * (ay - py) >= (ax - px) * (cy - py) && - (ax - px) * (by - py) >= (bx - px) * (ay - py) && - (bx - px) * (cy - py) >= (cx - px) * (by - py); -} + points[a_index].next_index = b_index; + points[a2_idx].prev_index = b2_idx; + points[a2_idx].next_index = an_idx; -/// @brief check if a diagonal between two polygon Points is valid -bool EarClipping::is_valid_diagonal(EarClipping::Point * a, EarClipping::Point * b) -{ - return a->next->i != b->i && a->prev->i != b->i && !intersects_polygon(a, b) && - ((locally_inside(a, b) && locally_inside(b, a) && middle_inside(a, b) && - (area(a->prev, a, b->prev) != 0.0 || area(a, b->prev, b) != 0.0)) || - (equals(a, b) && area(a->prev, a, a->next) > 0 && area(b->prev, b, b->next) > 0)); -} + points[b_index].prev_index = a_index; + points[an_idx].prev_index = b2_idx; + points[b2_idx].next_index = a2_idx; + points[b2_idx].prev_index = bp_idx; -/// @brief signed area of a triangle -double EarClipping::area( - const EarClipping::Point * p, const EarClipping::Point * q, const EarClipping::Point * r) -{ - return (q->y() - p->y()) * (r->x() - q->x()) - (q->x() - p->x()) * (r->y() - q->y()); -} + if (bp_idx != b_index) { + points[bp_idx].next_index = b2_idx; + } -/// @brief check if two points are equal -bool EarClipping::equals(const EarClipping::Point * p1, const EarClipping::Point * p2) -{ - return p1->x() == p2->x() && p1->y() == p2->y(); + return b2_idx; } -/// @brief check if two segments intersect -bool EarClipping::intersects( - const EarClipping::Point * p1, const EarClipping::Point * q1, const EarClipping::Point * p2, - const EarClipping::Point * q2) +std::size_t filter_points( + const std::size_t start_index, const std::size_t end_index, std::vector & points) { - int o1 = sign(area(p1, q1, p2)); - int o2 = sign(area(p1, q1, q2)); - int o3 = sign(area(p2, q2, p1)); - int o4 = sign(area(p2, q2, q1)); + auto p = start_index; + bool again = true; - if (o1 != o2 && o3 != o4) return true; + while (again && p != end_index) { + again = false; - if (o1 == 0 && on_segment(p1, p2, q1)) return true; - if (o2 == 0 && on_segment(p1, q2, q1)) return true; - if (o3 == 0 && on_segment(p2, p1, q2)) return true; - if (o4 == 0 && on_segment(p2, q1, q2)) return true; + if ( + !points[p].steiner && + (equals(p, points[p].next_index.value(), points) || + area(points, points[p].prev_index.value(), p, points[p].next_index.value()) == 0)) { + remove_point(p, points); + p = points[p].prev_index.value(); + + if (p == points[p].next_index.value()) { + break; + } + again = true; + } else { + p = points[p].next_index.value(); + } + } - return false; + return end_index; } -/// @brief for collinear points p, q, r, check if point q lies on segment pr -bool EarClipping::on_segment( - const EarClipping::Point * p, const EarClipping::Point * q, const EarClipping::Point * r) +std::size_t eliminate_hole( + const std::size_t hole_index, const std::size_t outer_index, std::vector & points) { - return q->x() <= std::max(p->x(), r->x()) && q->x() >= std::min(p->x(), r->x()) && - q->y() <= std::max(p->y(), r->y()) && q->y() >= std::min(p->y(), r->y()); + auto bridge = find_hole_bridge(hole_index, outer_index, points); + auto bridge_reverse = split_polygon(bridge, hole_index, points); + + auto next_index_bridge_reverse = points[bridge_reverse].next_index.value(); + filter_points(bridge_reverse, next_index_bridge_reverse, points); + + auto next_index_bridge = points[bridge].next_index.value(); + return filter_points(bridge, next_index_bridge, points); } -/// @brief Sign function for area calculation -int EarClipping::sign(double val) +std::size_t eliminate_holes( + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + std::vector & points) { - return (0.0 < val) - (val < 0.0); + std::vector queue; + + for (const auto & ring : inners) { + auto inner_index = linked_list(ring, false, vertices, points); + + if (points[inner_index].next_index.value() == inner_index) { + points[inner_index].steiner = true; + } + + queue.push_back(get_leftmost(inner_index, points)); + } + + std::sort(queue.begin(), queue.end(), [&](std::size_t a, std::size_t b) { + return points[a].x() < points[b].x(); + }); + + for (const auto & q : queue) { + outer_index = eliminate_hole(q, outer_index, points); + } + + return outer_index; } -/// @brief Check if a polygon diagonal intersects any polygon segments -bool EarClipping::intersects_polygon(const EarClipping::Point * a, const EarClipping::Point * b) +bool is_ear(const std::size_t ear_index, const std::vector & points) { - const EarClipping::Point * p = a; - do { + const auto a_index = points[ear_index].prev_index.value(); + const auto b_index = ear_index; + const auto c_index = points[ear_index].next_index.value(); + + const auto a = points[a_index]; + const auto b = points[b_index]; + const auto c = points[c_index]; + + if (area(points, a_index, b_index, c_index) >= 0) return false; + auto p_index = points[c_index].next_index.value(); + while (p_index != a_index) { + const auto p = points[p_index]; if ( - p->i != a->i && p->next->i != a->i && p->i != b->i && p->next->i != b->i && - intersects(p, p->next, a, b)) - return true; - p = p->next; - } while (p != a); + point_in_triangle(a.x(), a.y(), b.x(), b.y(), c.x(), c.y(), p.x(), p.y()) && + area(points, p.prev_index.value(), p_index, p.next_index.value()) >= 0) { + return false; + } + p_index = points[p_index].next_index.value(); + } - return false; + return true; } -/// @brief Check if a polygon diagonal is locally inside the polygon -bool EarClipping::locally_inside(const EarClipping::Point * a, const EarClipping::Point * b) +std::size_t cure_local_intersections( + std::size_t start_index, std::vector & indices, std::vector & points) { - return area(a->prev, a, a->next) < 0 ? area(a, b, a->next) >= 0 && area(a, a->prev, b) >= 0 - : area(a, b, a->prev) < 0 || area(a, a->next, b) < 0; + auto p = start_index; + bool updated = false; + + while (p != start_index || updated) { + updated = false; + auto a_idx = points[p].prev_index.value(); + auto b_idx = points[points[p].next_index.value()].next_index.value(); + + if ( + !equals(a_idx, b_idx, points) && + intersects( + a_idx, p, points[points[p].next_index.value()].next_index.value(), b_idx, points) && + locally_inside(a_idx, b_idx, points) && locally_inside(b_idx, a_idx, points)) { + indices.push_back(a_idx); + indices.push_back(p); + indices.push_back(b_idx); + + remove_point(p, points); + remove_point(points[p].next_index.value(), points); + + p = start_index = b_idx; + updated = true; + } else { + p = points[p].next_index.value(); + } + } + + return filter_points(p, p, points); } -/// @brief Check if the middle vertex of a polygon diagonal is inside the polygon -bool EarClipping::middle_inside(const EarClipping::Point * a, const EarClipping::Point * b) +void split_ear_clipping( + std::vector & points, const std::size_t start_idx, + std::vector & indices) { - const EarClipping::Point * p = a; - bool inside = false; - double px = (a->x() + b->x()) / 2; - double py = (a->y() + b->y()) / 2; + std::size_t a_idx = start_idx; do { - if ( - ((p->y() > py) != (p->next->y() > py)) && p->next->y() != p->y() && - (px < (p->next->x() - p->x()) * (py - p->y()) / (p->next->y() - p->y()) + p->x())) - inside = !inside; - p = p->next; - } while (p != a); + std::size_t b_idx = points[points[a_idx].next_index.value()].next_index.value(); + while (b_idx != points[a_idx].prev_index.value()) { + if (a_idx != b_idx && is_valid_diagonal(a_idx, b_idx, points)) { + std::size_t c_idx = split_polygon(a_idx, b_idx, points); - return inside; + a_idx = filter_points(start_idx, points[a_idx].next_index.value(), points); + c_idx = filter_points(start_idx, points[c_idx].next_index.value(), points); + + ear_clipping_linked(a_idx, indices, points); + ear_clipping_linked(c_idx, indices, points); + return; + } + b_idx = points[b_idx].next_index.value(); + } + a_idx = points[a_idx].next_index.value(); + } while (a_idx != start_idx); } -/// @brief Link two polygon vertices with a bridge -EarClipping::Point * EarClipping::split_polygon(EarClipping::Point * a, EarClipping::Point * b) +void ear_clipping_linked( + std::size_t ear_index, std::vector & indices, std::vector & points, + const int pass) { - Point2d a_point(a->x(), a->y()); - Point2d b_point(b->x(), b->y()); + auto stop = ear_index; + std::optional next = std::nullopt; - // Use construct_point to create new Point objects - EarClipping::Point * a2 = construct_point(a->i, a_point); - EarClipping::Point * b2 = construct_point(b->i, b_point); + while (points[ear_index].prev_index.value() != points[ear_index].next_index.value()) { + next = points[ear_index].next_index; - EarClipping::Point * an = a->next; - EarClipping::Point * bp = b->prev; + if (is_ear(ear_index, points)) { + indices.push_back(points[ear_index].prev_index.value()); + indices.push_back(ear_index); + indices.push_back(next.value()); - // Update the linked list connections - a->next = b; - b->prev = a; + remove_point(ear_index, points); - a2->next = an; - if (an) { - an->prev = a2; - } + ear_index = points[next.value()].next_index.value(); + stop = points[next.value()].next_index.value(); + continue; + } - b2->next = a2; - a2->prev = b2; + ear_index = next.value(); - if (bp) { - bp->next = b2; + if (ear_index == stop) { + if (pass == 0) { + ear_clipping_linked(filter_points(ear_index, ear_index, points), indices, points, 1); + } else if (pass == 1) { + ear_index = + cure_local_intersections(filter_points(ear_index, ear_index, points), indices, points); + ear_clipping_linked(ear_index, indices, points, 2); + } else if (pass == 2) { + split_ear_clipping(points, ear_index, indices); + } + break; + } } - b2->prev = bp; - - return b2; } -/// @brief create a Point and optionally link it with the previous one (in a circular doubly linked -/// list) -EarClipping::Point * EarClipping::insert_point( - std::size_t i, const Point2d & pt, EarClipping::Point * last) +std::vector perform_triangulation( + const alt::Polygon2d & polygon, std::vector & indices) { - EarClipping::Point * p = construct_point(static_cast(i), pt); + indices.clear(); + std::vector points; + std::size_t vertices = 0; + const auto & outer_ring = polygon.outer(); + std::size_t len = outer_ring.size(); + points.reserve(len * 3 / 2); - if (!last) { - p->prev = p; - p->next = p; - } else { - assert(last); - p->next = last->next; - p->prev = last; - last->next->prev = p; - last->next = p; + if (polygon.outer().empty()) return points; + + indices.reserve(len + outer_ring.size()); + auto outer_point_index = linked_list(outer_ring, true, vertices, points); + if ( + !points[outer_point_index].prev_index.has_value() || + outer_point_index == points[outer_point_index].prev_index.value()) { + return points; } - return p; -} -/// @brief remove a Point from the linked list -void EarClipping::remove_point(EarClipping::Point * p) -{ - p->next->prev = p->prev; - p->prev->next = p->next; + if (!polygon.inners().empty()) { + outer_point_index = eliminate_holes(polygon.inners(), outer_point_index, vertices, points); + } + + ear_clipping_linked(outer_point_index, indices, points); + return points; } -/// @brief main triangulate function -std::vector triangulate(const Polygon2d & poly) +std::vector triangulate(const alt::Polygon2d & poly) { - autoware::universe_utils::EarClipping triangulate; - triangulate(poly); - - std::vector triangles; + std::vector indices; + auto points = perform_triangulation(poly, indices); - const auto & indices = triangulate.indices; + std::vector triangles; const std::size_t num_indices = indices.size(); if (num_indices % 3 != 0) { throw std::runtime_error("Indices size should be a multiple of 3"); } - // Gather all vertices from outer and inner rings - std::vector all_vertices; - const auto & outer_ring = poly.outer(); - all_vertices.insert(all_vertices.end(), outer_ring.begin(), outer_ring.end()); - - for (const auto & inner_ring : poly.inners()) { - all_vertices.insert(all_vertices.end(), inner_ring.begin(), inner_ring.end()); - } - - const std::size_t total_vertices = all_vertices.size(); - for (std::size_t i = 0; i < num_indices; i += 3) { - if ( - indices[i] >= total_vertices || indices[i + 1] >= total_vertices || - indices[i + 2] >= total_vertices) { - throw std::runtime_error("Index out of range"); - } + alt::PointList2d vertices; + vertices.push_back(points[indices[i]].pt); + vertices.push_back(points[indices[i + 1]].pt); + vertices.push_back(points[indices[i + 2]].pt); + vertices.push_back(points[indices[i]].pt); - Polygon2d triangle; - triangle.outer().push_back(all_vertices[indices[i]]); - triangle.outer().push_back(all_vertices[indices[i + 1]]); - triangle.outer().push_back(all_vertices[indices[i + 2]]); - triangle.outer().push_back(all_vertices[indices[i]]); // Close the triangle + triangles.push_back(alt::ConvexPolygon2d::create(vertices).value()); + } + points.clear(); + return triangles; +} - triangles.push_back(triangle); +std::vector triangulate(const Polygon2d & poly) +{ + const auto alt_poly = alt::Polygon2d::create(poly); + if (!alt_poly.has_value()) { + return {}; } + const auto alt_triangles = triangulate(alt_poly.value()); + std::vector triangles; + for (const auto & alt_triangle : alt_triangles) { + triangles.push_back(alt_triangle.to_boost()); + } return triangles; } - } // namespace autoware::universe_utils - -#endif // EAR_CUT_IMPL_HPP diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp index 507be41dea7bb..0d360e2335df7 100644 --- a/common/autoware_universe_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -16,7 +16,6 @@ namespace autoware::universe_utils { - visualization_msgs::msg::Marker createDefaultMarker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, const int32_t type, const geometry_msgs::msg::Vector3 & scale, diff --git a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp index cc911f8848d03..90dfb1ede4701 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp @@ -35,30 +35,21 @@ TEST(alt_geometry, area) using autoware::universe_utils::alt::ConvexPolygon2d; using autoware::universe_utils::alt::Point2d; - { // Clockwise + { const Point2d p1 = {0.0, 0.0}; const Point2d p2 = {0.0, 7.0}; const Point2d p3 = {4.0, 2.0}; const Point2d p4 = {2.0, 0.0}; - const auto result = area(ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = area(ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_NEAR(result, 16.0, epsilon); } - - { // Counter-clockwise - const Point2d p1 = {0.0, 0.0}; - const Point2d p2 = {2.0, 0.0}; - const Point2d p3 = {4.0, 2.0}; - const Point2d p4 = {0.0, 7.0}; - const auto result = area(ConvexPolygon2d({p1, p2, p3, p4})); - - EXPECT_NEAR(result, -16.0, epsilon); - } } TEST(alt_geometry, convexHull) { using autoware::universe_utils::convex_hull; + using autoware::universe_utils::alt::PointList2d; using autoware::universe_utils::alt::Points2d; { @@ -76,21 +67,16 @@ TEST(alt_geometry, convexHull) points.push_back({2.9, 0.7}); const auto result = convex_hull(points); - ASSERT_EQ(result.vertices().size(), 7); - EXPECT_NEAR(result.vertices().at(0).x(), 2.0, epsilon); - EXPECT_NEAR(result.vertices().at(0).y(), 1.3, epsilon); - EXPECT_NEAR(result.vertices().at(1).x(), 2.4, epsilon); - EXPECT_NEAR(result.vertices().at(1).y(), 1.7, epsilon); - EXPECT_NEAR(result.vertices().at(2).x(), 4.1, epsilon); - EXPECT_NEAR(result.vertices().at(2).y(), 3.0, epsilon); - EXPECT_NEAR(result.vertices().at(3).x(), 5.3, epsilon); - EXPECT_NEAR(result.vertices().at(3).y(), 2.6, epsilon); - EXPECT_NEAR(result.vertices().at(4).x(), 5.4, epsilon); - EXPECT_NEAR(result.vertices().at(4).y(), 1.2, epsilon); - EXPECT_NEAR(result.vertices().at(5).x(), 4.9, epsilon); - EXPECT_NEAR(result.vertices().at(5).y(), 0.8, epsilon); - EXPECT_NEAR(result.vertices().at(6).x(), 2.9, epsilon); - EXPECT_NEAR(result.vertices().at(6).y(), 0.7, epsilon); + ASSERT_TRUE(result); + PointList2d ground_truth = {{2.0, 1.3}, {2.4, 1.7}, {4.1, 3.0}, {5.3, 2.6}, + {5.4, 1.2}, {4.9, 0.8}, {2.9, 0.7}, {2.0, 1.3}}; + ASSERT_EQ(result->vertices().size(), ground_truth.size()); + auto alt_it = result->vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != result->vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } } } @@ -98,46 +84,42 @@ TEST(alt_geometry, correct) { using autoware::universe_utils::correct; using autoware::universe_utils::alt::ConvexPolygon2d; - using autoware::universe_utils::alt::Points2d; + using autoware::universe_utils::alt::PointList2d; { // Correctly oriented - Points2d vertices; + PointList2d vertices; vertices.push_back({1.0, 1.0}); vertices.push_back({1.0, -1.0}); vertices.push_back({-1.0, -1.0}); vertices.push_back({-1.0, 1.0}); - ConvexPolygon2d poly(vertices); - correct(poly); - - ASSERT_EQ(poly.vertices().size(), 4); - EXPECT_NEAR(poly.vertices().at(0).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(0).y(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).y(), 1.0, epsilon); + auto poly = ConvexPolygon2d::create(vertices).value(); // correct()-ed in create() + + PointList2d ground_truth = {{1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}}; + ASSERT_EQ(poly.vertices().size(), ground_truth.size()); + auto alt_it = poly.vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != poly.vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } } { // Wrongly oriented - Points2d vertices; + PointList2d vertices; vertices.push_back({1.0, 1.0}); vertices.push_back({-1.0, 1.0}); - vertices.push_back({1.0, -1.0}); vertices.push_back({-1.0, -1.0}); - ConvexPolygon2d poly(vertices); - correct(poly); - - ASSERT_EQ(poly.vertices().size(), 4); - EXPECT_NEAR(poly.vertices().at(0).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(0).y(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).y(), 1.0, epsilon); + vertices.push_back({1.0, -1.0}); + auto poly = ConvexPolygon2d::create(vertices).value(); // correct()-ed in create() + + PointList2d ground_truth = {{1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}}; + ASSERT_EQ(poly.vertices().size(), ground_truth.size()); + auto alt_it = poly.vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != poly.vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } } } @@ -153,7 +135,7 @@ TEST(alt_geometry, coveredBy) const Point2d p2 = {1.0, -1.0}; const Point2d p3 = {-1.0, -1.0}; const Point2d p4 = {-1.0, 1.0}; - const auto result = covered_by(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -164,7 +146,7 @@ TEST(alt_geometry, coveredBy) const Point2d p2 = {2.0, 1.0}; const Point2d p3 = {1.0, 1.0}; const Point2d p4 = {1.0, 2.0}; - const auto result = covered_by(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -175,7 +157,7 @@ TEST(alt_geometry, coveredBy) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {0.0, -1.0}; const Point2d p4 = {0.0, 1.0}; - const auto result = covered_by(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -196,8 +178,9 @@ TEST(alt_geometry, disjoint) const Point2d p6 = {4.0, 4.0}; const Point2d p7 = {6.0, 2.0}; const Point2d p8 = {4.0, 0.0}; - const auto result = - disjoint(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = disjoint( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -211,8 +194,9 @@ TEST(alt_geometry, disjoint) const Point2d p6 = {2.0, 4.0}; const Point2d p7 = {4.0, 2.0}; const Point2d p8 = {2.0, 0.0}; - const auto result = - disjoint(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = disjoint( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -298,7 +282,7 @@ TEST(alt_geometry, distance) const Point2d p2 = {3.0, -1.0}; const Point2d p3 = {1.0, -1.0}; const Point2d p4 = {1.0, 1.0}; - const auto result = distance(p, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = distance(p, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_NEAR(result, 1.0, epsilon); } @@ -309,12 +293,50 @@ TEST(alt_geometry, distance) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {-1.0, -1.0}; const Point2d p4 = {-1.0, 1.0}; - const auto result = distance(p, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = distance(p, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_NEAR(result, 0.0, epsilon); } } +TEST(geometry, envelope) +{ + using autoware::universe_utils::envelope; + using autoware::universe_utils::alt::PointList2d; + using autoware::universe_utils::alt::Polygon2d; + + { + const auto poly = Polygon2d::create( + PointList2d{ + {2.0, 1.3}, + {2.4, 1.7}, + {2.8, 1.8}, + {3.4, 1.2}, + {3.7, 1.6}, + {3.4, 2.0}, + {4.1, 3.0}, + {5.3, 2.6}, + {5.4, 1.2}, + {4.9, 0.8}, + {2.9, 0.7}, + {2.0, 1.3}}, + {PointList2d{{4.0, 2.0}, {4.2, 1.4}, {4.8, 1.9}, {4.4, 2.2}, {4.0, 2.0}}}) + .value(); + const auto result = envelope(poly); + + ASSERT_TRUE(result); + + PointList2d ground_truth = {{2.0, 0.7}, {2.0, 3.0}, {5.4, 3.0}, {5.4, 0.7}, {2.0, 0.7}}; + ASSERT_EQ(result->vertices().size(), ground_truth.size()); + auto alt_it = result->vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != result->vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } + } +} + TEST(alt_geometry, intersects) { using autoware::universe_utils::intersects; @@ -420,8 +442,9 @@ TEST(alt_geometry, intersects) const Point2d p6 = {2.0, 0.0}; const Point2d p7 = {0.0, 0.0}; const Point2d p8 = {0.0, 2.0}; - const auto result = - intersects(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = intersects( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -435,8 +458,9 @@ TEST(alt_geometry, intersects) const Point2d p6 = {3.0, 2.0}; const Point2d p7 = {2.0, 2.0}; const Point2d p8 = {2.0, 3.0}; - const auto result = - intersects(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = intersects( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -450,8 +474,9 @@ TEST(alt_geometry, intersects) const Point2d p6 = {2.0, 1.0}; const Point2d p7 = {1.0, 1.0}; const Point2d p8 = {1.0, 2.0}; - const auto result = - intersects(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = intersects( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -493,30 +518,52 @@ TEST(alt_geometry, isAbove) TEST(alt_geometry, isClockwise) { using autoware::universe_utils::is_clockwise; - using autoware::universe_utils::alt::ConvexPolygon2d; - using autoware::universe_utils::alt::Point2d; + using autoware::universe_utils::alt::PointList2d; { // Clockwise - const Point2d p1 = {0.0, 0.0}; - const Point2d p2 = {0.0, 7.0}; - const Point2d p3 = {4.0, 2.0}; - const Point2d p4 = {2.0, 0.0}; - const auto result = is_clockwise(ConvexPolygon2d({p1, p2, p3, p4})); + PointList2d vertices; + vertices.push_back({0.0, 0.0}); + vertices.push_back({0.0, 7.0}); + vertices.push_back({4.0, 2.0}); + vertices.push_back({2.0, 0.0}); + const auto result = is_clockwise(vertices); EXPECT_TRUE(result); } { // Counter-clockwise - const Point2d p1 = {0.0, 0.0}; - const Point2d p2 = {2.0, 0.0}; - const Point2d p3 = {4.0, 2.0}; - const Point2d p4 = {0.0, 7.0}; - const auto result = is_clockwise(ConvexPolygon2d({p1, p2, p3, p4})); + PointList2d vertices; + vertices.push_back({0.0, 0.0}); + vertices.push_back({2.0, 0.0}); + vertices.push_back({4.0, 2.0}); + vertices.push_back({0.0, 7.0}); + const auto result = is_clockwise(vertices); EXPECT_FALSE(result); } } +TEST(geometry, simplify) +{ + using autoware::universe_utils::simplify; + using autoware::universe_utils::alt::PointList2d; + + { + const PointList2d points = {{1.1, 1.1}, {2.5, 2.1}, {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9}}; + const double max_distance = 0.5; + const auto result = simplify(points, max_distance); + + PointList2d ground_truth = {{1.1, 1.1}, {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9}}; + ASSERT_EQ(result.size(), ground_truth.size()); + auto alt_it = result.begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != result.end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } + } +} + TEST(alt_geometry, touches) { using autoware::universe_utils::touches; @@ -568,7 +615,7 @@ TEST(alt_geometry, touches) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {0.0, -1.0}; const Point2d p4 = {0.0, 1.0}; - const auto result = touches(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = touches(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -579,7 +626,7 @@ TEST(alt_geometry, touches) const Point2d p2 = {2.0, 1.0}; const Point2d p3 = {1.0, 1.0}; const Point2d p4 = {1.0, 2.0}; - const auto result = touches(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = touches(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -597,7 +644,7 @@ TEST(alt_geometry, within) const Point2d p2 = {1.0, -1.0}; const Point2d p3 = {-1.0, -1.0}; const Point2d p4 = {-1.0, 1.0}; - const auto result = within(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -608,7 +655,7 @@ TEST(alt_geometry, within) const Point2d p2 = {2.0, 1.0}; const Point2d p3 = {1.0, 1.0}; const Point2d p4 = {1.0, 2.0}; - const auto result = within(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -619,7 +666,7 @@ TEST(alt_geometry, within) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {0.0, -1.0}; const Point2d p4 = {0.0, 1.0}; - const auto result = within(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -633,8 +680,9 @@ TEST(alt_geometry, within) const Point2d p6 = {2.0, -2.0}; const Point2d p7 = {-2.0, -2.0}; const Point2d p8 = {-2.0, 2.0}; - const auto result = - within(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = within( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -648,8 +696,9 @@ TEST(alt_geometry, within) const Point2d p6 = {3.0, 2.0}; const Point2d p7 = {2.0, 2.0}; const Point2d p8 = {2.0, 3.0}; - const auto result = - within(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = within( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -663,8 +712,9 @@ TEST(alt_geometry, within) const Point2d p6 = {1.0, -1.0}; const Point2d p7 = {-1.0, -1.0}; const Point2d p8 = {-1.0, 1.0}; - const auto result = - within(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = within( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -673,7 +723,7 @@ TEST(alt_geometry, within) TEST(alt_geometry, areaRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -691,7 +741,8 @@ TEST(alt_geometry, areaRand) const auto ground_truth = boost::geometry::area(polygons[i]); ground_truth_area_ns += sw.toc(); - const auto alt_poly = autoware::universe_utils::alt::from_boost(polygons[i]); + const auto alt_poly = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); sw.tic(); const auto alt = autoware::universe_utils::area(alt_poly); alt_area_ns += sw.toc(); @@ -712,7 +763,7 @@ TEST(alt_geometry, areaRand) TEST(alt_geometry, convexHullRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -735,21 +786,19 @@ TEST(alt_geometry, convexHullRand) boost::geometry::convex_hull(outer, ground_truth); ground_truth_hull_ns += sw.toc(); - const auto vertices = autoware::universe_utils::alt::from_boost(polygons[i]).vertices(); + const auto vertices = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value().vertices(); sw.tic(); - const auto alt = autoware::universe_utils::convex_hull(vertices); + const auto alt = autoware::universe_utils::convex_hull({vertices.begin(), vertices.end()}); alt_hull_ns += sw.toc(); - if (ground_truth.outer().size() - 1 != alt.vertices().size()) { - std::cout << "Alt failed for the polygon: "; - std::cout << boost::geometry::wkt(polygons[i]) << std::endl; - } - ASSERT_EQ( - ground_truth.outer().size() - 1, - alt.vertices().size()); // alt::ConvexPolygon2d does not have closing point - for (size_t i = 0; i < alt.vertices().size(); ++i) { - EXPECT_NEAR(ground_truth.outer().at(i).x(), alt.vertices().at(i).x(), epsilon); - EXPECT_NEAR(ground_truth.outer().at(i).y(), alt.vertices().at(i).y(), epsilon); + ASSERT_TRUE(alt); + ASSERT_EQ(ground_truth.outer().size(), alt->vertices().size()); + auto ground_truth_it = ground_truth.outer().begin(); + auto alt_it = alt->vertices().begin(); + for (; ground_truth_it != ground_truth.outer().end(); ++ground_truth_it, ++alt_it) { + EXPECT_NEAR(ground_truth_it->x(), alt_it->x(), epsilon); + EXPECT_NEAR(ground_truth_it->y(), alt_it->y(), epsilon); } } std::printf("polygons_nb = %d, vertices = %ld\n", polygons_nb, vertices); @@ -762,7 +811,7 @@ TEST(alt_geometry, convexHullRand) TEST(alt_geometry, coveredByRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -790,8 +839,9 @@ TEST(alt_geometry, coveredByRand) ground_truth_not_covered_ns += sw.toc(); } - const auto alt_point = autoware::universe_utils::alt::from_boost(point); - const auto alt_poly = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_point = autoware::universe_utils::alt::Point2d(point); + const auto alt_poly = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::covered_by(alt_point, alt_poly); if (alt) { @@ -828,7 +878,7 @@ TEST(alt_geometry, coveredByRand) TEST(alt_geometry, disjointRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -855,8 +905,10 @@ TEST(alt_geometry, disjointRand) ground_truth_not_disjoint_ns += sw.toc(); } - const auto alt_poly1 = autoware::universe_utils::alt::from_boost(polygons[i]); - const auto alt_poly2 = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_poly1 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); + const auto alt_poly2 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::disjoint(alt_poly1, alt_poly2); if (alt) { @@ -892,7 +944,7 @@ TEST(alt_geometry, disjointRand) TEST(alt_geometry, intersectsRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -919,8 +971,10 @@ TEST(alt_geometry, intersectsRand) ground_truth_no_intersect_ns += sw.toc(); } - const auto alt_poly1 = autoware::universe_utils::alt::from_boost(polygons[i]); - const auto alt_poly2 = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_poly1 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); + const auto alt_poly2 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::intersects(alt_poly1, alt_poly2); if (alt) { @@ -956,7 +1010,7 @@ TEST(alt_geometry, intersectsRand) TEST(alt_geometry, touchesRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -984,8 +1038,9 @@ TEST(alt_geometry, touchesRand) ground_truth_not_touching_ns += sw.toc(); } - const auto alt_point = autoware::universe_utils::alt::from_boost(point); - const auto alt_poly = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_point = autoware::universe_utils::alt::Point2d(point); + const auto alt_poly = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::touches(alt_point, alt_poly); if (alt) { @@ -1022,7 +1077,7 @@ TEST(alt_geometry, touchesRand) TEST(alt_geometry, withinPolygonRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -1049,8 +1104,10 @@ TEST(alt_geometry, withinPolygonRand) ground_truth_not_within_ns += sw.toc(); } - const auto alt_poly1 = autoware::universe_utils::alt::from_boost(polygons[i]); - const auto alt_poly2 = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_poly1 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); + const auto alt_poly2 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::within(alt_poly1, alt_poly2); if (alt) { diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index ab5e9f4236bad..af90ab1f32c57 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -1937,7 +1937,7 @@ TEST( TEST(geometry, intersectPolygonRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -2227,7 +2227,7 @@ TEST(geometry, intersectConcavePolygonRand) { std::vector polygons; std::vector> triangulations; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp deleted file mode 100644 index 1275b47346d78..0000000000000 --- a/common/interpolation/src/spline_interpolation.cpp +++ /dev/null @@ -1,289 +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 "interpolation/spline_interpolation.hpp" - -#include - -namespace -{ -// solve Ax = d -// where A is tridiagonal matrix -// [b_0 c_0 ... ] -// [a_0 b_1 c_1 ... O ] -// A = [ ... ] -// [ O ... a_N-3 b_N-2 c_N-2] -// [ ... a_N-2 b_N-1] -struct TDMACoef -{ - explicit TDMACoef(const size_t num_row) - { - a.resize(num_row - 1); - b.resize(num_row); - c.resize(num_row - 1); - d.resize(num_row); - } - - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; - -inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef) -{ - const auto & a = tdma_coef.a; - const auto & b = tdma_coef.b; - const auto & c = tdma_coef.c; - const auto & d = tdma_coef.d; - - const size_t num_row = b.size(); - - std::vector x(num_row); - if (num_row != 1) { - // calculate p and q - std::vector p; - std::vector q; - p.push_back(-c[0] / b[0]); - q.push_back(d[0] / b[0]); - - for (size_t i = 1; i < num_row; ++i) { - const double den = b[i] + a[i - 1] * p[i - 1]; - p.push_back(-c[i - 1] / den); - q.push_back((d[i] - a[i - 1] * q[i - 1]) / den); - } - - // calculate solution - x[num_row - 1] = q[num_row - 1]; - - for (size_t i = 1; i < num_row; ++i) { - const size_t j = num_row - 1 - i; - x[j] = p[j] * x[j + 1] + q[j]; - } - } else { - x[0] = (d[0] / b[0]); - } - - return x; -} -} // namespace - -namespace interpolation -{ -std::vector spline( - const std::vector & base_keys, const std::vector & base_values, - const std::vector & query_keys) -{ - // calculate spline coefficients - SplineInterpolation interpolator(base_keys, base_values); - - // interpolate base_keys at query_keys - return interpolator.getSplineInterpolatedValues(query_keys); -} - -std::vector splineByAkima( - const std::vector & base_keys, const std::vector & base_values, - const std::vector & query_keys) -{ - constexpr double epsilon = 1e-5; - - // calculate m - std::vector m_values; - for (size_t i = 0; i < base_keys.size() - 1; ++i) { - const double m_val = - (base_values.at(i + 1) - base_values.at(i)) / (base_keys.at(i + 1) - base_keys.at(i)); - m_values.push_back(m_val); - } - - // calculate s - std::vector s_values; - for (size_t i = 0; i < base_keys.size(); ++i) { - if (i == 0) { - s_values.push_back(m_values.front()); - continue; - } else if (i == base_keys.size() - 1) { - s_values.push_back(m_values.back()); - continue; - } else if (i == 1 || i == base_keys.size() - 2) { - const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0; - s_values.push_back(s_val); - continue; - } - - const double denom = std::abs(m_values.at(i + 1) - m_values.at(i)) + - std::abs(m_values.at(i - 1) - m_values.at(i - 2)); - if (std::abs(denom) < epsilon) { - const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0; - s_values.push_back(s_val); - continue; - } - - const double s_val = (std::abs(m_values.at(i + 1) - m_values.at(i)) * m_values.at(i - 1) + - std::abs(m_values.at(i - 1) - m_values.at(i - 2)) * m_values.at(i)) / - denom; - s_values.push_back(s_val); - } - - // calculate cubic coefficients - std::vector a; - std::vector b; - std::vector c; - std::vector d; - for (size_t i = 0; i < base_keys.size() - 1; ++i) { - a.push_back( - (s_values.at(i) + s_values.at(i + 1) - 2.0 * m_values.at(i)) / - std::pow(base_keys.at(i + 1) - base_keys.at(i), 2)); - b.push_back( - (3.0 * m_values.at(i) - 2.0 * s_values.at(i) - s_values.at(i + 1)) / - (base_keys.at(i + 1) - base_keys.at(i))); - c.push_back(s_values.at(i)); - d.push_back(base_values.at(i)); - } - - // interpolate - std::vector res; - size_t j = 0; - for (const auto & query_key : query_keys) { - while (base_keys.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys.at(j); - res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); - } - return res; -} -} // namespace interpolation - -void SplineInterpolation::calcSplineCoefficients( - const std::vector & base_keys, const std::vector & base_values) -{ - // throw exceptions for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); - - const size_t num_base = base_keys.size(); // N+1 - - std::vector diff_keys; // N - std::vector diff_values; // N - for (size_t i = 0; i < num_base - 1; ++i) { - diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i)); - diff_values.push_back(base_values.at(i + 1) - base_values.at(i)); - } - - std::vector v = {0.0}; - if (num_base > 2) { - // solve tridiagonal matrix algorithm - TDMACoef tdma_coef(num_base - 2); // N-1 - - for (size_t i = 0; i < num_base - 2; ++i) { - tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]); - if (i != num_base - 3) { - tdma_coef.a[i] = diff_keys[i + 1]; - tdma_coef.c[i] = diff_keys[i + 1]; - } - tdma_coef.d[i] = - 6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]); - } - - const std::vector tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef); - - // calculate v - v.insert(v.end(), tdma_res.begin(), tdma_res.end()); - } - v.push_back(0.0); - - // calculate a, b, c, d of spline coefficients - multi_spline_coef_ = interpolation::MultiSplineCoef{num_base - 1}; // N - for (size_t i = 0; i < num_base - 1; ++i) { - multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; - multi_spline_coef_.b[i] = v[i] / 2.0; - multi_spline_coef_.c[i] = - diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0; - multi_spline_coef_.d[i] = base_values[i]; - } - - base_keys_ = base_keys; -} - -std::vector SplineInterpolation::getSplineInterpolatedValues( - const std::vector & query_keys) const -{ - // throw exceptions for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - const auto & d = multi_spline_coef_.d; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); - } - - return res; -} - -std::vector SplineInterpolation::getSplineInterpolatedDiffValues( - const std::vector & query_keys) const -{ - // throw exceptions for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(c.at(j) + (2.0 * b.at(j) + 3.0 * a.at(j) * ds) * ds); - } - - return res; -} - -std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( - const std::vector & query_keys) const -{ - // throw exceptions for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds); - } - - return res; -} diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 31892853a855f..b03270133a52c 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -95,10 +95,14 @@ bool transformObjects( tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); } for (auto & object : output_msg.objects) { - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); tf_target2objects = tf_target2objects_world * tf_objects_world2objects; - tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); - // TODO(yukkysaito) transform covariance + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); } } return true; diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index d0802bbc9b457..a3f4a99300716 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_perception_msgs autoware_universe_utils geometry_msgs - interpolation libboost-dev pcl_conversions pcl_ros diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index e26c9e6a7e1ea..96c62d9f50323 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -14,9 +14,9 @@ #include "object_recognition_utils/predicted_path_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spherical_linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include @@ -69,13 +69,13 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( } const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_time, input, resampled_time); + return autoware::interpolation::lerp(input_time, input, resampled_time); }; const auto spline = [&](const auto & input) { - return interpolation::spline(input_time, input, resampled_time); + return autoware::interpolation::spline(input_time, input, resampled_time); }; const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_time, input, resampled_time); + return autoware::interpolation::slerp(input_time, input, resampled_time); }; const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp index 2368e07432031..b6637a3c619a5 100644 --- a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -99,7 +99,6 @@ void BirdEyeViewController::reset() y_property_->setFloat(0); } -// cppcheck-suppress unusedFunction void BirdEyeViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) { if (event.shift()) { diff --git a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp index 85c8a81ea7932..d427f63628d69 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp @@ -44,7 +44,6 @@ PoseHistory::PoseHistory() : last_stamp_(0, 0, RCL_ROS_TIME) PoseHistory::~PoseHistory() = default; // Properties are deleted by Qt -// cppcheck-suppress unusedFunction void PoseHistory::onInitialize() { MFDClass::onInitialize(); 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 662ce97b162cd..391ebdc0c1c77 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 @@ -95,13 +95,6 @@ void PoseHistoryFootprint::update_vehicle_info() } } -void PoseHistoryFootprint::update_visualization() -{ - if (last_msg_ptr_) { - processMessage(last_msg_ptr_); - } -} - void PoseHistoryFootprint::onInitialize() { MFDClass::onInitialize(); diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp index 241e682924fad..7634f6049b6a0 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -61,7 +61,6 @@ class PoseHistoryFootprint void update_footprint(); private Q_SLOTS: - void update_visualization(); void update_vehicle_info(); private: // NOLINT : suppress redundancy warnings diff --git a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp index c8fde566c5211..42f36ed6f4c08 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp @@ -92,19 +92,16 @@ void PoseWithCovarianceHistory::onInitialize() lines_ = std::make_unique(scene_manager_, scene_node_); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::onEnable() { subscribe(); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::onDisable() { unsubscribe(); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::update(float wall_dt, float ros_dt) { (void)wall_dt; @@ -151,7 +148,6 @@ void PoseWithCovarianceHistory::update_shape_type() property_arrow_color_->setHidden(!is_arrow); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::processMessage( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr message) { diff --git a/common/tier4_state_rviz_plugin/src/custom_button.cpp b/common/tier4_state_rviz_plugin/src/custom_button.cpp index 1a887497a2903..a2d4b92e17621 100644 --- a/common/tier4_state_rviz_plugin/src/custom_button.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_button.cpp @@ -63,7 +63,6 @@ QSize CustomElevatedButton::minimumSizeHint() const return sizeHint(); } -// cppcheck-suppress unusedFunction void CustomElevatedButton::updateStyle( const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, const QColor & pressedColor, const QColor & checkedColor, const QColor & disabledBgColor, diff --git a/common/tier4_state_rviz_plugin/src/custom_container.cpp b/common/tier4_state_rviz_plugin/src/custom_container.cpp index 4e7bafd044897..681cebbefd492 100644 --- a/common/tier4_state_rviz_plugin/src/custom_container.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_container.cpp @@ -26,7 +26,6 @@ CustomContainer::CustomContainer(QWidget * parent) : QFrame(parent), cornerRadiu setLayout(layout); } -// cppcheck-suppress unusedFunction QGridLayout * CustomContainer::getLayout() const { return layout; // Provide access to the layout diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp index 786b58ecf1d2e..8063bdc0fbc28 100644 --- a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -40,7 +40,6 @@ CustomSegmentedButtonItem * CustomSegmentedButton::addButton(const QString & tex return button; } -// cppcheck-suppress unusedFunction QButtonGroup * CustomSegmentedButton::getButtonGroup() const { return buttonGroup; diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp index d0ede8ec1dfe6..cdd3aa0d25263 100644 --- a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -44,14 +44,12 @@ CustomSegmentedButtonItem::CustomSegmentedButtonItem(const QString & text, QWidg // setFixedSize(width, height); // } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setHovered(bool hovered) { isHovered = hovered; updateCheckableState(); } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setCheckableButton(bool checkable) { setCheckable(checkable); @@ -66,14 +64,12 @@ void CustomSegmentedButtonItem::updateCheckableState() update(); } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setDisabledButton(bool disabled) { isDisabled = disabled; updateCheckableState(); } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setActivated(bool activated) { isActivated = activated; diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 66d4483fbdf84..42b3f4c9f96de 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -56,7 +56,7 @@ We do not activate AEB module if it satisfies the following conditions. - Ego vehicle is not in autonomous driving state -- When the ego vehicle is not moving (Current Velocity is very low) +- When the ego vehicle is not moving (Current Velocity is below a 0.1 m/s threshold) ### 2. Generate a predicted path of the ego vehicle @@ -112,6 +112,14 @@ Once all target obstacles have been identified, the AEB module chooses the point ### 4. Obstacle velocity estimation +To begin calculating the target point's velocity, the point must enter the speed calculation area, +which is defined by the `speed_calculation_expansion_margin` parameter. +Depending on the operational environment, +this margin can reduce unnecessary autonomous emergency braking +caused by velocity miscalculations during the initial calculation steps. + +![speed_calculation_expansion](./image/speed_calculation_expansion.drawio.svg) + Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object relative speed using the following equations: $$ @@ -188,30 +196,36 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t ## Parameters -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| publish_debug_markers | [-] | bool | flag to publish debug markers | true | -| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | -| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | -| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | -| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | -| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | -| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | +| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | +| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | +| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | +| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | +| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | +| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| speed_calculation_expansion_margin | [m] | double | expansion width of the ego vehicle for the beginning speed calculation | 0.1 | ## Limitations diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index d5c6356c38897..f7548536beaef 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -7,7 +7,8 @@ use_predicted_object_data: false use_object_velocity_calculation: true check_autoware_state: true - min_generated_path_length: 0.5 + min_generated_imu_path_length: 0.5 + max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 4.5 @@ -27,6 +28,7 @@ # Point cloud cropping expand_width: 0.1 path_footprint_extra_margin: 4.0 + speed_calculation_expansion_margin: 0.5 # Point cloud clustering cluster_tolerance: 0.15 #[m] diff --git a/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg new file mode 100644 index 0000000000000..37728253370e4 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 9addc059ad0af..214a6dc309210 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -86,6 +88,7 @@ struct ObjectData double velocity{0.0}; double rss{0.0}; double distance_to_object{0.0}; + bool is_target{true}; }; /** @@ -341,6 +344,7 @@ class AEB : public rclcpp::Node rclcpp::Publisher::SharedPtr info_marker_publisher_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; + rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; // timer rclcpp::TimerBase::SharedPtr timer_; mutable std::shared_ptr time_keeper_{nullptr}; @@ -435,12 +439,14 @@ class AEB : public rclcpp::Node * @brief Create object data using point cloud clusters * @param ego_path Ego vehicle path * @param ego_polys Polygons representing the ego vehicle footprint + * @param speed_calc_ego_polys Polygons representing the expanded ego vehicle footprint for speed + * calculation area * @param stamp Timestamp of the data * @param objects Vector to store the created object data * @param obstacle_points_ptr Pointer to the point cloud of obstacles */ void getClosestObjectsOnPath( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects); /** @@ -496,7 +502,7 @@ class AEB : public rclcpp::Node * @param debug_markers Marker array for debugging */ void addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); /** @@ -550,12 +556,14 @@ class AEB : public rclcpp::Node bool use_object_velocity_calculation_; bool check_autoware_state_; double path_footprint_extra_margin_; + double speed_calculation_expansion_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; double voxel_grid_x_; double voxel_grid_y_; double voxel_grid_z_; - double min_generated_path_length_; + double min_generated_imu_path_length_; + double max_generated_imu_path_length_; double expand_width_; double longitudinal_offset_; double t_response_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 5a2dacad2dfc9..18f3716b755ee 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -42,6 +42,7 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; @@ -105,6 +106,14 @@ std::optional getTransform( */ void fillMarkerFromPolygon( const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon3d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); } // namespace autoware::motion::control::autonomous_emergency_braking::utils #endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 173d419bf6d9a..02c8d0e6aaf83 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -41,6 +41,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs visualization_msgs diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 1f8fd6f4137a2..b3988ba238fe7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -59,6 +59,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -71,6 +72,16 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +void appendPointToPolygon(Polygon3d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point3d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + point.z() = geom_point.z; + + bg::append(polygon.outer(), point); +} + Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) @@ -128,6 +139,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) this->create_publisher("~/debug/obstacle_pointcloud", 1); debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); info_marker_publisher_ = this->create_publisher("~/info/markers", 1); + debug_rss_distance_publisher_ = + this->create_publisher("~/debug/rss_distance", 1); } // Diagnostics { @@ -144,13 +157,16 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); check_autoware_state_ = declare_parameter("check_autoware_state"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); + speed_calculation_expansion_margin_ = + declare_parameter("speed_calculation_expansion_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = declare_parameter("detection_range_max_height_margin"); voxel_grid_x_ = declare_parameter("voxel_grid_x"); voxel_grid_y_ = declare_parameter("voxel_grid_y"); voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); + min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); + max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); longitudinal_offset_ = declare_parameter("longitudinal_offset"); t_response_ = declare_parameter("t_response"); @@ -204,13 +220,16 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "check_autoware_state", check_autoware_state_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + updateParam( + parameters, "speed_calculation_expansion_margin", speed_calculation_expansion_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); updateParam(parameters, "voxel_grid_x", voxel_grid_x_); updateParam(parameters, "voxel_grid_y", voxel_grid_y_); updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); + updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); updateParam(parameters, "longitudinal_offset", longitudinal_offset_); updateParam(parameters, "t_response", t_response_); @@ -444,10 +463,11 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto ego_polys = generatePathFootprint(path, expand_width_); std::vector objects; // Crop out Pointcloud using an extra wide ego path - if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { + if ( + use_pointcloud_data_ && points_belonging_to_cluster_hulls && + !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - getClosestObjectsOnPath( - path, ego_polys, current_time, points_belonging_to_cluster_hulls, objects); + getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } if (use_predicted_object_data_) { createObjectDataUsingPredictedObjects(path, ego_polys, objects); @@ -462,39 +482,41 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return objects; }; - auto check_collision = [&]( - const Path & path, const colorTuple & debug_colors, - const std::string & debug_ns, - PointCloud::Ptr points_belonging_to_cluster_hulls) { - time_keeper_->start_track("has_collision_with_" + debug_ns); - auto objects = - get_objects_on_path(path, points_belonging_to_cluster_hulls, debug_colors, debug_ns); - // Get only the closest object and calculate its speed + auto check_collision = [&](const Path & path, std::vector & objects) { + time_keeper_->start_track("has_collision"); const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = + // Attempt to find the closest object + const auto closest_object_itr = std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) { + // target objects have priority + if (o1.is_target != o2.is_target) { + return o1.is_target; + } return o1.distance_to_object < o2.distance_to_object; }); - if (closest_object_point_itr == objects.end()) { - return std::nullopt; - } - const auto closest_object_speed = (use_object_velocity_calculation_) - ? collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v) - : std::make_optional(0.0); - if (!closest_object_speed.has_value()) { - return std::nullopt; + if (closest_object_itr != objects.end()) { + // Calculate speed for the closest object + const auto closest_object_speed = (use_object_velocity_calculation_) + ? collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_itr, path, current_v) + : std::make_optional(0.0); + + if (closest_object_speed.has_value()) { + closest_object_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_itr); + } } - closest_object_point_itr->velocity = closest_object_speed.value(); - return std::make_optional(*closest_object_point_itr); + + return std::nullopt; }); - const bool has_collision = (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + const bool has_collision = + (closest_object_point.has_value() && closest_object_point.value().is_target) + ? hasCollision(current_v, closest_object_point.value()) + : false; - time_keeper_->end_track("has_collision_with_" + debug_ns); + time_keeper_->end_track("has_collision"); // check collision using rss distance return has_collision; }; @@ -529,27 +551,46 @@ bool AEB::checkCollision(MarkerArray & debug_markers) getPointsBelongingToClusterHulls( filtered_objects, points_belonging_to_cluster_hulls, debug_markers); - const auto has_collision_imu_path = - [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { - if (!use_imu_path_ || !angular_velocity_ptr_) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; - const std::string ns = "imu"; - return check_collision(ego_imu_path, debug_color, ns, points_belonging_to_cluster_hulls); - }; + const auto imu_path_objects = (!use_imu_path_ || !angular_velocity_ptr_) + ? std::vector{} + : get_objects_on_path( + ego_imu_path, points_belonging_to_cluster_hulls, + {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}, "imu"); + + const auto mpc_path_objects = + (!use_predicted_trajectory_ || !predicted_traj_ptr_ || !ego_mpc_path.has_value()) + ? std::vector{} + : get_objects_on_path( + ego_mpc_path.value(), points_belonging_to_cluster_hulls, + {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}, "mpc"); + + // merge object data which comes from the ego (imu) path and predicted path + auto merge_objects = + [&](const std::vector & imu_objects, const std::vector & mpc_objects) { + std::vector merged_objects = imu_objects; + merged_objects.insert(merged_objects.end(), mpc_objects.begin(), mpc_objects.end()); + return merged_objects; + }; + + auto merged_imu_mpc_objects = merge_objects(imu_path_objects, mpc_path_objects); + if (merged_imu_mpc_objects.empty()) return false; - // step4. make function to check collision with predicted trajectory from control module - const auto has_collision_mpc_path = - [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { - if (!use_predicted_trajectory_ || !ego_mpc_path.has_value()) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; - const std::string ns = "mpc"; - return check_collision( - ego_mpc_path.value(), debug_color, ns, points_belonging_to_cluster_hulls); + // merge path points for the collision checking + auto merge_paths = [&](const std::optional & mpc_path, const Path & imu_path) { + if (!mpc_path.has_value()) { + return imu_path; + } + Path merged_path = imu_path; // Start with imu_path + merged_path.insert( + merged_path.end(), mpc_path.value().begin(), mpc_path.value().end()); // Append mpc_path + return merged_path; }; - // evaluate if there is a collision for both paths - const bool has_collision = has_collision_imu_path(points_belonging_to_cluster_hulls) || - has_collision_mpc_path(points_belonging_to_cluster_hulls); + auto merge_imu_mpc_path = merge_paths(ego_mpc_path, ego_imu_path); + if (merge_imu_mpc_path.empty()) return false; + + // evaluate if there is a collision for merged (imu and mpc) paths + const bool has_collision = check_collision(merge_imu_mpc_path, merged_imu_mpc_objects); // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { @@ -575,12 +616,16 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object return ego_stopping_distance + obj_braking_distance + longitudinal_offset_; }); + tier4_debug_msgs::msg::Float32Stamped rss_distance_msg; + rss_distance_msg.stamp = get_clock()->now(); + rss_distance_msg.data = rss_dist; + debug_rss_distance_publisher_->publish(rss_distance_msg); + if (closest_object.distance_to_object > rss_dist) return false; // collision happens ObjectData collision_data = closest_object; collision_data.rss = rss_dist; - collision_data.distance_to_object = closest_object.distance_to_object; collision_data_keeper_.setCollisionData(collision_data); return true; } @@ -596,39 +641,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - - if (std::abs(curr_v) < 0.1) { - // if current velocity is too small, assume it stops at the same point + const double & dt = imu_prediction_time_interval_; + const double distance_between_points = curr_v * dt; + constexpr double minimum_distance_between_points{1e-2}; + // if current velocity is too small, assume it stops at the same point + // if distance between points is too small, arc length calculation is unreliable, so we skip + // creating the path + if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { return path; } - constexpr double epsilon = std::numeric_limits::epsilon(); - const double & dt = imu_prediction_time_interval_; const double & horizon = imu_prediction_time_horizon_; - for (double t = 0.0; t < horizon + epsilon; t += dt) { - curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; - curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { - continue; - } - path.push_back(current_pose); - } + double path_arc_length = 0.0; + double t = 0.0; - // If path is shorter than minimum path length - while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { + bool finished_creating_path = false; + while (!finished_creating_path) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { - continue; - } + + t += dt; + path_arc_length += distance_between_points; + + finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); + finished_creating_path = + (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_); path.push_back(current_pose); } return path; @@ -648,9 +689,18 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); + constexpr double minimum_distance_between_points{1e-2}; for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); + + // skip points that are too close to the last point in the path + if ( + autoware::universe_utils::calcDistance2d(path.back(), map_pose) < + minimum_distance_between_points) { + continue; + } + path.push_back(map_pose); if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { @@ -658,13 +708,16 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) } } time_keeper_->end_track("createPath"); - return path; + return (!path.empty()) ? std::make_optional(path) : std::nullopt; } void AEB::generatePathFootprint( const Path & path, const double extra_width_margin, std::vector & polygons) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return; + } for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -674,8 +727,11 @@ void AEB::generatePathFootprint( std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - std::vector polygons; autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return {}; + } + std::vector polygons; for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -688,7 +744,7 @@ void AEB::createObjectDataUsingPredictedObjects( std::vector & object_data_vector) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (predicted_objects_ptr_->objects.empty()) return; + if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return; const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; const auto & objects = predicted_objects_ptr_->objects; @@ -787,7 +843,7 @@ void AEB::getPointsBelongingToClusterHulls( ec.extract(cluster_idx); return cluster_idx; }); - std::vector hull_polygons; + std::vector hull_polygons; for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); bool cluster_surpasses_threshold_height{false}; @@ -806,7 +862,7 @@ void AEB::getPointsBelongingToClusterHulls( std::vector polygons; PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); - Polygon2d hull_polygon; + Polygon3d hull_polygon; for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); if (publish_debug_markers_) { @@ -823,12 +879,12 @@ void AEB::getPointsBelongingToClusterHulls( } void AEB::getClosestObjectsOnPath( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || points_belonging_to_cluster_hulls->empty()) { + // check if the predicted path has a valid number of points + if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; } @@ -845,6 +901,19 @@ void AEB::getClosestObjectsOnPath( autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; + // calculate the lateral offset between the ego vehicle and the object + const double lateral_offset = + std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position)); + + if (std::isnan(lateral_offset)) continue; + + // object is outside region of interest + if ( + lateral_offset > + vehicle_info_.vehicle_width_m / 2.0 + expand_width_ + speed_calculation_expansion_margin_) { + continue; + } + // If the object is behind the ego, we need to use the backward long offset. The distance should // be a positive number in any case const bool is_object_in_front_of_ego = obj_arc_length > 0.0; @@ -857,14 +926,8 @@ void AEB::getClosestObjectsOnPath( obj.position = obj_position; obj.velocity = 0.0; obj.distance_to_object = std::abs(dist_ego_to_object); - - const Point2d obj_point(p.x, p.y); - for (const auto & ego_poly : ego_polys) { - if (bg::within(obj_point, ego_poly)) { - objects.push_back(obj); - break; - } - } + obj.is_target = (lateral_offset < vehicle_info_.vehicle_width_m / 2.0 + expand_width_); + objects.push_back(obj); } } @@ -872,6 +935,9 @@ void AEB::cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (ego_polys.empty()) { + return; + } PointCloud::Ptr full_points_ptr(new PointCloud); pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); // Create a Point cloud with the points of the ego footprint @@ -900,7 +966,7 @@ void AEB::cropPointCloudWithEgoFootprintPath( } void AEB::addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 9c8bbe388fdc5..5d9782ada5fbd 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -186,4 +186,21 @@ void fillMarkerFromPolygon( } } +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z()); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + } // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 11c2fb1ed671d..c2a58941cc144 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -116,6 +116,8 @@ TEST_F(TestAEB, checkCollision) ObjectData object_collision; object_collision.distance_to_object = 0.5; object_collision.velocity = 0.1; + object_collision.position.x = 1.0; + object_collision.position.y = 1.0; ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); ObjectData object_no_collision; @@ -161,8 +163,7 @@ TEST_F(TestAEB, checkImuPathGeneration) aeb_node_->getPointsBelongingToClusterHulls( obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); std::vector objects; - aeb_node_->getClosestObjectsOnPath( - imu_path, footprint, stamp, points_belonging_to_cluster_hulls, objects); + aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects); ASSERT_FALSE(objects.empty()); } @@ -170,7 +171,7 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) { const double dt = aeb_node_->imu_prediction_time_interval_; const double horizon = aeb_node_->imu_prediction_time_horizon_; - const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_; const double slow_velocity = min_generated_path_length / (2.0 * horizon); constexpr double yaw_rate = 0.05; const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); @@ -184,6 +185,26 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) ASSERT_TRUE(footprint.size() == imu_path.size() - 1); } +TEST_F(TestAEB, checkImuPathGenerationIsCut) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_; + const double fast_velocity = 2.0 * max_generated_path_length / (horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + constexpr double epsilon{1e-3}; + ASSERT_TRUE( + autoware::motion_utils::calcArcLength(imu_path) <= + max_generated_path_length + dt * fast_velocity + epsilon); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) { const double velocity = 0.0; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 27107cfd8974b..9ef803d874520 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -160,11 +160,6 @@ class LaneDepartureChecker const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); - std::vector createVehicleFootprints( - const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, - const Param & param); - std::vector createVehicleFootprints(const PathWithLaneId & path) const; - static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp deleted file mode 100644 index 4435f282054ad..0000000000000 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ - -#include -#include -#include - -#include - -struct FootprintMargin -{ - double lon; - double lat; -}; - -inline FootprintMargin calcFootprintMargin( - const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale) -{ - const auto Cov_in_map = covariance.covariance; - Eigen::Matrix2d Cov_xy_map; - Cov_xy_map << Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1], Cov_in_map[1 * 6 + 0], - Cov_in_map[1 * 6 + 1]; - - const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation); - - // To get a position in a transformed coordinate, rotate the inverse direction - Eigen::Matrix2d R_map2vehicle; - R_map2vehicle << std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle), std::sin(-yaw_vehicle), - std::cos(-yaw_vehicle); - // Rotate covariance E((X, Y)^t*(X, Y)) = E(R*(x,y)*(x,y)^t*R^t) - // when Rotate point (X, Y)^t= R*(x, y)^t. - const Eigen::Matrix2d Cov_xy_vehicle = R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose(); - - // The longitudinal/lateral length is represented - // in Cov_xy_vehicle(0,0), Cov_xy_vehicle(1,1) respectively. - return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; -} - -#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index ed44a3f777c65..45a651339cc12 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp @@ -15,15 +15,22 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ +#include +#include + #include #include +#include +#include #include namespace autoware::lane_departure_checker::utils { +using autoware::universe_utils::LinearRing2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; /** @@ -42,6 +49,30 @@ TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double * @note this function assumes the input trajectory is sampled dense enough */ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double interval); + +/** + * @brief create vehicle footprints along the trajectory with the given covariance and margin + * @param covariance vehicle pose with covariance + * @param trajectory trajectory along which the vehicle footprints are created + * @param vehicle_info vehicle information + * @param footprint_margin_scale scale of the footprint margin + * @return vehicle footprints along the trajectory + */ +std::vector createVehicleFootprints( + const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_margin_scale); + +/** + * @brief create vehicle footprints along the path with the given margin + * @param path path along which the vehicle footprints are created + * @param vehicle_info vehicle information + * @param footprint_extra_margin extra margin for the footprint + * @return vehicle footprints along the path + */ +std::vector createVehicleFootprints( + const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_extra_margin); } // namespace autoware::lane_departure_checker::utils #endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f775c3e32c7af..a6b91c25763c5 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -14,7 +14,6 @@ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" -#include "autoware/lane_departure_checker/util/create_vehicle_footprint.hpp" #include "autoware/lane_departure_checker/utils.hpp" #include @@ -123,8 +122,9 @@ Output LaneDepartureChecker::update(const Input & input) braking_distance); output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); } - output.vehicle_footprints = - createVehicleFootprints(input.current_odom->pose, output.resampled_trajectory, param_); + output.vehicle_footprints = utils::createVehicleFootprints( + input.current_odom->pose, output.resampled_trajectory, *vehicle_info_ptr_, + param_.footprint_margin_scale); output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); @@ -163,7 +163,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - std::vector vehicle_footprints = createVehicleFootprints(path); + std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); return willLeaveLane(candidate_lanelets, vehicle_footprints); } @@ -177,43 +178,6 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } -std::vector LaneDepartureChecker::createVehicleFootprints( - const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, - const Param & param) -{ - // Calculate longitudinal and lateral margin based on covariance - const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale); - - // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(margin.lat, margin.lon); - - // Create vehicle footprint on each TrajectoryPoint - std::vector vehicle_footprints; - for (const auto & p : trajectory) { - vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); - } - - return vehicle_footprints; -} - -std::vector LaneDepartureChecker::createVehicleFootprints( - const PathWithLaneId & path) const -{ - // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = - vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); - - // Create vehicle footprint on each Path point - std::vector vehicle_footprints; - for (const auto & p : path.points) { - vehicle_footprints.push_back(transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); - } - - return vehicle_footprints; -} - std::vector LaneDepartureChecker::createVehiclePassingAreas( const std::vector & vehicle_footprints) { @@ -249,7 +213,8 @@ std::vector> LaneDepartureChecker::getLanele universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // Get Footprint Hull basic polygon - std::vector vehicle_footprints = createVehicleFootprints(path); + std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); lanelet::BasicPolygon2d footprint_hull_basic_polygon = toBasicPolygon2D(footprint_hull); @@ -331,7 +296,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check if the footprint is not fully contained within the fused lanelets polygon - const std::vector vehicle_footprints = createVehicleFootprints(path); + const std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); if (!fused_lanelets_polygon) return true; return !std::all_of( @@ -348,7 +314,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const std::vector vehicle_footprints = createVehicleFootprints(path); + const std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); auto is_all_footprints_within = [&](const auto & polygon) { return std::all_of( @@ -380,7 +347,8 @@ PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( PathWithLaneId temp_path; const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); if (path.points.empty() || !fused_lanelets_polygon) return temp_path; - const auto vehicle_footprints = createVehicleFootprints(path); + const auto vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); { universe_utils::ScopedTimeTrack st2( diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp index 1bbb06ce2f524..324936c633158 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp @@ -18,6 +18,38 @@ #include +namespace +{ +struct FootprintMargin +{ + double lon; + double lat; +}; + +FootprintMargin calcFootprintMargin( + const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale) +{ + const auto Cov_in_map = covariance.covariance; + Eigen::Matrix2d Cov_xy_map; + Cov_xy_map << Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1], Cov_in_map[1 * 6 + 0], + Cov_in_map[1 * 6 + 1]; + + const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation); + + // To get a position in a transformed coordinate, rotate the inverse direction + Eigen::Matrix2d R_map2vehicle; + R_map2vehicle << std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle), std::sin(-yaw_vehicle), + std::cos(-yaw_vehicle); + // Rotate covariance E((X, Y)^t*(X, Y)) = E(R*(x,y)*(x,y)^t*R^t) + // when Rotate point (X, Y)^t= R*(x, y)^t. + const Eigen::Matrix2d Cov_xy_vehicle = R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose(); + + // The longitudinal/lateral length is represented + // in Cov_xy_vehicle(0,0), Cov_xy_vehicle(1,1) respectively. + return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; +} +} // namespace + namespace autoware::lane_departure_checker::utils { TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double length) @@ -91,4 +123,42 @@ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double return resampled; } + +std::vector createVehicleFootprints( + const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_margin_scale) +{ + // Calculate longitudinal and lateral margin based on covariance + const auto margin = calcFootprintMargin(covariance, footprint_margin_scale); + + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = vehicle_info.createFootprint(margin.lat, margin.lon); + + // Create vehicle footprint on each TrajectoryPoint + std::vector vehicle_footprints; + for (const auto & p : trajectory) { + vehicle_footprints.push_back( + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); + } + + return vehicle_footprints; +} + +std::vector createVehicleFootprints( + const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_extra_margin) +{ + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = vehicle_info.createFootprint(footprint_extra_margin); + + // Create vehicle footprint on each Path point + std::vector vehicle_footprints; + for (const auto & p : path.points) { + vehicle_footprints.push_back(transformVector( + local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); + } + + return vehicle_footprints; +} } // namespace autoware::lane_departure_checker::utils diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp new file mode 100644 index 0000000000000..6189d8803a3ea --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp @@ -0,0 +1,280 @@ +// Copyright 2024 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 "autoware/lane_departure_checker/utils.hpp" + +#include + +#include + +using autoware::universe_utils::LinearRing2d; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::PoseWithCovariance; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using TrajectoryPoints = std::vector; + +namespace +{ +PoseWithCovariance create_pose_with_covariance( + const Eigen::Matrix2d & covariance_xy, const double yaw) +{ + PoseWithCovariance pose_with_covariance; + pose_with_covariance.covariance[0 * 6 + 0] = covariance_xy(0, 0); + pose_with_covariance.covariance[0 * 6 + 1] = covariance_xy(0, 1); + pose_with_covariance.covariance[1 * 6 + 0] = covariance_xy(1, 0); + pose_with_covariance.covariance[1 * 6 + 1] = covariance_xy(1, 1); + pose_with_covariance.pose.orientation.z = std::sin(yaw / 2); + pose_with_covariance.pose.orientation.w = std::cos(yaw / 2); + return pose_with_covariance; +} + +TrajectoryPoints create_trajectory_points( + const std::vector> & xy_yaws) +{ + TrajectoryPoints trajectory_points; + for (const auto & [xy, yaw] : xy_yaws) { + TrajectoryPoint p; + p.pose.position.x = xy(0); + p.pose.position.y = xy(1); + p.pose.orientation.z = std::sin(yaw / 2); + p.pose.orientation.w = std::cos(yaw / 2); + trajectory_points.push_back(p); + } + return trajectory_points; +} + +PathWithLaneId create_path(const std::vector> & xy_yaws) +{ + PathWithLaneId path; + for (const auto & [xy, yaw] : xy_yaws) { + PathPointWithLaneId p; + p.point.pose.position.x = xy(0); + p.point.pose.position.y = xy(1); + p.point.pose.orientation.z = std::sin(yaw / 2); + p.point.pose.orientation.w = std::cos(yaw / 2); + path.points.push_back(p); + } + return path; +} + +// reference: +// https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml +constexpr double wheel_radius_m = 0.383; +constexpr double wheel_width_m = 0.235; +constexpr double wheel_base_m = 2.79; +constexpr double wheel_tread_m = 1.64; +constexpr double front_overhang_m = 1.0; +constexpr double rear_overhang_m = 1.1; +constexpr double left_overhang_m = 0.128; +constexpr double right_overhang_m = 0.128; +constexpr double vehicle_height_m = 2.5; +constexpr double max_steer_angle_rad = 0.70; +} // namespace + +struct CreateVehicleFootprintsAlongTrajectoryParam +{ + std::string description; + Eigen::Matrix2d covariance_xy; + double yaw; + std::vector> trajectory_points; + double footprint_margin_scale; + std::vector expected_footprints; +}; + +std::ostream & operator<<(std::ostream & os, const CreateVehicleFootprintsAlongTrajectoryParam & p) +{ + return os << p.description; +} + +struct CreateVehicleFootprintsAlongPathParam +{ + std::string description; + std::vector> path_points; + double footprint_extra_margin; + std::vector expected_footprints; +}; + +std::ostream & operator<<(std::ostream & os, const CreateVehicleFootprintsAlongPathParam & p) +{ + return os << p.description; +} + +class CreateVehicleFootprintsAlongTrajectoryTest +: public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_info = autoware::vehicle_info_utils::createVehicleInfo( + wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m, + left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad); + } + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +class CreateVehicleFootprintsAlongPathTest +: public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_info = autoware::vehicle_info_utils::createVehicleInfo( + wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m, + left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad); + } + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +TEST_P(CreateVehicleFootprintsAlongTrajectoryTest, test_create_vehicle_footprints) +{ + const auto p = GetParam(); + const auto pose_with_covariance = create_pose_with_covariance(p.covariance_xy, p.yaw); + const auto trajectory_points = create_trajectory_points(p.trajectory_points); + const auto footprints = autoware::lane_departure_checker::utils::createVehicleFootprints( + pose_with_covariance, trajectory_points, vehicle_info, p.footprint_margin_scale); + + ASSERT_EQ(footprints.size(), p.expected_footprints.size()); + for (size_t i = 0; i < footprints.size(); ++i) { + const auto & footprint = footprints.at(i); + const auto & expected_footprint = p.expected_footprints.at(i); + ASSERT_EQ(footprint.size(), expected_footprint.size()); + for (size_t j = 0; j < footprint.size(); ++j) { + EXPECT_DOUBLE_EQ(footprint.at(j).x(), expected_footprint.at(j).x()); + EXPECT_DOUBLE_EQ(footprint.at(j).y(), expected_footprint.at(j).y()); + } + } +} + +TEST_P(CreateVehicleFootprintsAlongPathTest, test_create_vehicle_footprints) +{ + const auto p = GetParam(); + const auto path = create_path(p.path_points); + const auto footprints = autoware::lane_departure_checker::utils::createVehicleFootprints( + path, vehicle_info, p.footprint_extra_margin); + + ASSERT_EQ(footprints.size(), p.expected_footprints.size()); + for (size_t i = 0; i < footprints.size(); ++i) { + const auto & footprint = footprints.at(i); + const auto & expected_footprint = p.expected_footprints.at(i); + ASSERT_EQ(footprint.size(), expected_footprint.size()); + for (size_t j = 0; j < footprint.size(); ++j) { + EXPECT_DOUBLE_EQ(footprint.at(j).x(), expected_footprint.at(j).x()); + EXPECT_DOUBLE_EQ(footprint.at(j).y(), expected_footprint.at(j).y()); + } + } +} + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CreateVehicleFootprintsAlongTrajectoryTest, + ::testing::Values( + CreateVehicleFootprintsAlongTrajectoryParam{ + "EmptyTrajectory", Eigen::Matrix2d{{0.0, 0.0}, {0.0, 0.0}}, 0.0, {}, 0.0, {}}, + CreateVehicleFootprintsAlongTrajectoryParam{ + "SinglePointTrajectory", + Eigen::Matrix2d{{0.0, 0.0}, {0.0, 0.0}}, + 0.0, + {{{0.0, 0.0}, 0.0}}, + 0.0, + {{{front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, wheel_tread_m / 2.0 + left_overhang_m}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}}}}, + CreateVehicleFootprintsAlongTrajectoryParam{ + "NonZeroMargin", + Eigen::Matrix2d{{0.1, 0.0}, {0.0, 0.2}}, + 0.0, + {{{0.0, 0.0}, 0.0}, {{1.0, 0.0}, 0.0}}, + 1.0, + {{{front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}}, + {{front_overhang_m + wheel_base_m + 0.1 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0 + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + wheel_tread_m / 2.0 + left_overhang_m + 0.2}}}}, + CreateVehicleFootprintsAlongTrajectoryParam{ + "NonZeroYaw", + Eigen::Matrix2d{{0.2, 0.0}, {0.0, 0.1}}, + M_PI_2, + {{{0.0, 0.0}, 0.0}, {{1.0, 0.0}, 0.0}}, + 1.0, + {{{front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}}, + {{front_overhang_m + wheel_base_m + 0.1 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0 + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + wheel_tread_m / 2.0 + left_overhang_m + 0.2}}}}), + ::testing::PrintToStringParamName()); + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CreateVehicleFootprintsAlongPathTest, + ::testing::Values( + CreateVehicleFootprintsAlongPathParam{"EmptyTrajectory", {}, 0.0, {}}, + CreateVehicleFootprintsAlongPathParam{ + "SinglePointTrajectory", + {{{0.0, 0.0}, 0.0}}, + 0.0, + {{{front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, wheel_tread_m / 2.0 + left_overhang_m}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}}}}, + CreateVehicleFootprintsAlongPathParam{ + "NonZeroMargin", + {{{0.0, 0.0}, 0.0}, {{1.0, 0.0}, 0.0}}, + 0.1, + {{{front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1), -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1), wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.1}}, + {{front_overhang_m + wheel_base_m + 0.1 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {wheel_base_m / 2.0 + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1) + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1) + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {wheel_base_m / 2.0 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + wheel_tread_m / 2.0 + left_overhang_m + 0.1}}}}), + ::testing::PrintToStringParamName()); diff --git a/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp index 782b986f6e556..8fa91bc8782e8 100644 --- a/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp +++ b/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp @@ -70,7 +70,7 @@ TEST_P(CutTrajectoryTest, test_cut_trajectory) } INSTANTIATE_TEST_SUITE_P( - CutTrajectoryTests, CutTrajectoryTest, + LaneDepartureCheckerTest, CutTrajectoryTest, ::testing::Values( CutTrajectoryTestParam{ "EmptyTrajectory", std::vector{}, 1.0, std::vector{}}, diff --git a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp index ac89882a39493..67fe69323584e 100644 --- a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp +++ b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp @@ -71,7 +71,7 @@ TEST_P(ResampleTrajectoryTest, test_resample_trajectory) } INSTANTIATE_TEST_SUITE_P( - ResampleTrajectoryTests, ResampleTrajectoryTest, + LaneDepartureCheckerTest, ResampleTrajectoryTest, ::testing::Values( ResampleTrajectoryTestParam{"EmptyTrajectory", {}, 1.0, {}}, ResampleTrajectoryTestParam{"SinglePointTrajectory", {{1.0, 0.0, 0.0}}, 1.0, {{1.0, 0.0, 0.0}}}, diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index e25352797a0d0..5c5f8886d6ed3 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -28,7 +29,6 @@ diagnostic_updater eigen geometry_msgs - interpolation osqp_interface rclcpp rclcpp_components diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index b5cd0e7ba3e2a..0f350dc40ad0e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -14,10 +14,10 @@ #include "autoware/mpc_lateral_controller/mpc.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/mpc_lateral_controller/mpc_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "interpolation/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -383,8 +383,8 @@ std::pair MPC::updateStateForDelayCompensation( double k, v = 0.0; try { // NOTE: When driving backward, the curvature's sign should be reversed. - k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; - v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); + k = autoware::interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; + v = autoware::interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); } catch (const std::exception & e) { RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); return {false, {}}; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 4ff7f329e5498..f4ba74d74f246 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -104,7 +104,6 @@ MpcLateralController::MpcLateralController( m_mpc->setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - m_mpc->setVehicleModel(vehicle_model_ptr); auto qpsolver_ptr = createQPSolverInterface(node); m_mpc->setQPSolver(qpsolver_ptr); diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 37eb47ab0396e..c257bada0b0b2 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" #include #include @@ -134,10 +134,10 @@ std::pair resampleMPCTrajectoryByDistance( convertEulerAngleToMonotonic(input_yaw); const auto lerp_arc_length = [&](const auto & input_value) { - return interpolation::lerp(input_arclength, input_value, output_arclength); + return autoware::interpolation::lerp(input_arclength, input_value, output_arclength); }; const auto spline_arc_length = [&](const auto & input_value) { - return interpolation::spline(input_arclength, input_value, output_arclength); + return autoware::interpolation::spline(input_arclength, input_value, output_arclength); }; output.x = spline_arc_length(input.x); @@ -165,7 +165,7 @@ bool linearInterpMPCTrajectory( convertEulerAngleToMonotonic(in_traj_yaw); const auto lerp_arc_length = [&](const auto & input_value) { - return interpolation::lerp(in_index, input_value, out_index); + return autoware::interpolation::lerp(in_index, input_value, out_index); }; try { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp index a36a0b4eefccd..52cab33c048ac 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spherical_linear_interpolation.hpp" #include "tf2/utils.h" #include @@ -99,22 +99,22 @@ std::pair lerpTrajectoryPoint( { const size_t i = seg_idx; - interpolated_point.pose.position.x = interpolation::lerp( + interpolated_point.pose.position.x = autoware::interpolation::lerp( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); - interpolated_point.pose.position.y = interpolation::lerp( + interpolated_point.pose.position.y = autoware::interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.position.z = interpolation::lerp( + interpolated_point.pose.position.z = autoware::interpolation::lerp( points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio); - interpolated_point.pose.orientation = interpolation::lerpOrientation( + interpolated_point.pose.orientation = autoware::interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, interpolate_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio); - interpolated_point.acceleration_mps2 = interpolation::lerp( + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio); - interpolated_point.heading_rate_rps = interpolation::lerp( + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); } diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 91d77e454b7ff..86dab79a74559 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -30,7 +31,6 @@ diagnostic_updater eigen geometry_msgs - interpolation rclcpp rclcpp_components std_msgs diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 5eb6c87e063eb..134de5d1ac8bd 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -176,10 +176,11 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const auto p0 = trajectory.points.at(i).pose; const auto p1 = trajectory.points.at(i + 1).pose; p = trajectory.points.at(i).pose; - p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio); - p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio); - p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio); - p.orientation = interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); + p.position.x = autoware::interpolation::lerp(p0.position.x, p1.position.x, ratio); + p.position.y = autoware::interpolation::lerp(p0.position.y, p1.position.y, ratio); + p.position.z = autoware::interpolation::lerp(p0.position.z, p1.position.z, ratio); + p.orientation = + autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); break; } remain_dist -= dist; diff --git a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 3c547e762bce6..a898727ded3d1 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" -#include "interpolation/spherical_linear_interpolation.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -303,7 +303,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +311,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +320,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +328,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +336,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index fa8f5847d6c31..548129eb06f8c 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_universe_utils @@ -30,7 +31,6 @@ diagnostic_updater eigen geometry_msgs - interpolation osqp_interface rclcpp rclcpp_components 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 b359eb16387db..16e383916eb6d 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 @@ -19,9 +19,9 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" +#include #include #include -#include #include #include diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 878136d837678..a088b4a4a168e 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -27,6 +27,7 @@ autoware_control_msgs autoware_motion_utils autoware_planning_msgs + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -36,7 +37,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing std_msgs tf2 tf2_eigen diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 77d6df2021c8f..984584d16aa8f 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 48640b21b6064..4b6d0755db25f 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -119,26 +119,26 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; } else { - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = - interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation - interpolated_point.heading_rate_rps = - interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation - interpolated_point.front_wheel_angle_rad = interpolation::lerp( + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = - interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation - const double interpolated_time = interpolation::lerp( + const double interpolated_time = autoware::interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index ef3522b215227..b47c4d99c8dd2 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 20c39c0b58f2e..1cd48aee74469 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -23,6 +23,7 @@ autoware_geo_pose_projector autoware_gyro_odometer autoware_lidar_marker_localizer + autoware_ndt_scan_matcher autoware_pointcloud_preprocessor autoware_pose_estimator_arbiter autoware_pose_initializer @@ -30,7 +31,6 @@ eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt - ndt_scan_matcher topic_tools yabloc_common yabloc_image_processing diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml index 7f33fb0b946d1..e0f4c9319d0d5 100644 --- a/localization/autoware_ekf_localizer/package.xml +++ b/localization/autoware_ekf_localizer/package.xml @@ -23,11 +23,11 @@ eigen autoware_kalman_filter + autoware_localization_util autoware_universe_utils diagnostic_msgs fmt geometry_msgs - localization_util nav_msgs rclcpp rclcpp_components diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index f678412a25438..1b747c750977c 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -17,7 +17,7 @@ #include "autoware/ekf_localizer/diagnostics.hpp" #include "autoware/ekf_localizer/string.hpp" #include "autoware/ekf_localizer/warning_message.hpp" -#include "localization_util/covariance_ellipse.hpp" +#include "autoware/localization_util/covariance_ellipse.hpp" #include #include diff --git a/localization/autoware_gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml index 0e78d0dea51b6..77de1d582fc0f 100644 --- a/localization/autoware_gyro_odometer/package.xml +++ b/localization/autoware_gyro_odometer/package.xml @@ -17,11 +17,11 @@ ament_cmake_auto autoware_cmake + autoware_localization_util autoware_universe_utils diagnostic_msgs fmt geometry_msgs - localization_util rclcpp rclcpp_components sensor_msgs diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index bc2abb4a8a321..9511f168f346f 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -72,7 +72,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); - diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + diagnostics_ = + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 1b2c4246a037a..036b3d7cab527 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -15,10 +15,10 @@ #ifndef GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER_CORE_HPP_ +#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/transform_listener.hpp" -#include "localization_util/diagnostics_module.hpp" #include @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml index e8597a6fc9a4e..4f6726abb33ef 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -19,9 +19,9 @@ ament_index_cpp aruco autoware_landmark_manager + autoware_localization_util cv_bridge diagnostic_msgs - localization_util rclcpp rclcpp_components tf2_eigen diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 80845402a59f3..2418cf3f5dd1c 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -44,7 +44,7 @@ #include "ar_tag_based_localizer.hpp" -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" #include #include @@ -94,7 +94,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); return; } - ekf_pose_buffer_ = std::make_unique( + ekf_pose_buffer_ = std::make_unique( this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); /* @@ -168,8 +168,8 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; // get self pose - const std::optional interpolate_result = - ekf_pose_buffer_->interpolate(sensor_stamp); + const std::optional + interpolate_result = ekf_pose_buffer_->interpolate(sensor_stamp); if (!interpolate_result) { return; } diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index 9826c04e3a86f..ddfdf852be2b5 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -46,7 +46,7 @@ #define AR_TAG_BASED_LOCALIZER_HPP_ #include "autoware/landmark_manager/landmark_manager.hpp" -#include "localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" #include @@ -122,7 +122,7 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; - std::unique_ptr ekf_pose_buffer_; + std::unique_ptr ekf_pose_buffer_; landmark_manager::LandmarkManager landmark_manager_; }; diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index d69adc7904411..43ef73b8c5226 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -19,10 +19,10 @@ eigen autoware_lanelet2_extension + autoware_localization_util autoware_map_msgs autoware_universe_utils geometry_msgs - localization_util rclcpp tf2_eigen diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index 488f250e325d2..04823a71febe9 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -14,9 +14,9 @@ #include "autoware/landmark_manager/landmark_manager.hpp" +#include "autoware/localization_util/util_func.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" -#include "localization_util/util_func.hpp" #include #include @@ -178,12 +178,14 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( } if (consider_orientation) { - const Eigen::Affine3d landmark_pose = pose_to_affine3d(mapped_landmark_on_map); + const Eigen::Affine3d landmark_pose = + autoware::localization_util::pose_to_affine3d(mapped_landmark_on_map); const Eigen::Affine3d landmark_to_base_link = - pose_to_affine3d(detected_landmark_on_base_link).inverse(); + autoware::localization_util::pose_to_affine3d(detected_landmark_on_base_link).inverse(); const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link; - const Pose new_self_pose = matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); + const Pose new_self_pose = + autoware::localization_util::matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); // update min_distance = curr_distance; diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml index bf60d96311ff1..2b19b46b55424 100755 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml @@ -15,10 +15,10 @@ autoware_cmake autoware_landmark_manager + autoware_localization_util autoware_map_msgs autoware_point_types autoware_universe_utils - localization_util pcl_conversions pcl_ros rclcpp diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 83fcae2352f51..06d2acf8a55fc 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -81,7 +81,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti param_.save_file_name = this->declare_parameter("save_file_name"); param_.save_frame_id = this->declare_parameter("save_frame_id"); - ekf_pose_buffer_ = std::make_unique( + ekf_pose_buffer_ = std::make_unique( this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m); rclcpp::CallbackGroup::SharedPtr points_callback_group; @@ -122,7 +122,8 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_, this, false); - diagnostics_module_.reset(new DiagnosticsModule(this, "marker_detection_status")); + diagnostics_module_.reset( + new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() @@ -195,8 +196,8 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin } // (2) get Self Pose - const std::optional interpolate_result = - ekf_pose_buffer_->interpolate(sensor_ros_time); + const std::optional + interpolate_result = ekf_pose_buffer_->interpolate(sensor_ros_time); const bool is_received_self_pose = interpolate_result != std::nullopt; diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose); @@ -607,7 +608,7 @@ void LidarMarkerLocalizer::transform_sensor_measurement( const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = autoware::universe_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = - pose_to_matrix4f(target_to_source_pose_stamped.pose); + autoware::localization_util::pose_to_matrix4f(target_to_source_pose_stamped.pose); pcl_ros::transformPointCloud( base_to_sensor_matrix, *sensor_points_input_ptr, *sensor_points_output_ptr); } diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index 11a6e0b34abdc..d1482c6912592 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_MARKER_LOCALIZER_HPP_ #define LIDAR_MARKER_LOCALIZER_HPP_ -#include "localization_util/diagnostics_module.hpp" -#include "localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" #include @@ -134,11 +134,11 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_module_; + std::shared_ptr diagnostics_module_; Param param_; bool is_activated_; - std::unique_ptr ekf_pose_buffer_; + std::unique_ptr ekf_pose_buffer_; landmark_manager::LandmarkManager landmark_manager_; }; diff --git a/localization/autoware_localization_error_monitor/package.xml b/localization/autoware_localization_error_monitor/package.xml index 426b0d6b6c432..463e7aee347d7 100644 --- a/localization/autoware_localization_error_monitor/package.xml +++ b/localization/autoware_localization_error_monitor/package.xml @@ -19,9 +19,9 @@ autoware_cmake eigen + autoware_localization_util autoware_universe_utils diagnostic_msgs - localization_util nav_msgs rclcpp_components tf2 diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 3f9a02834c74d..63edbe5f6a9c7 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -58,7 +58,8 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); - diagnostics_error_monitor_ = std::make_unique(this, "ellipse_error_status"); + diagnostics_error_monitor_ = + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index 49abacfa60f38..7b26573964b38 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -15,8 +15,8 @@ #ifndef LOCALIZATION_ERROR_MONITOR_HPP_ #define LOCALIZATION_ERROR_MONITOR_HPP_ -#include "localization_util/covariance_ellipse.hpp" -#include "localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/covariance_ellipse.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" #include #include @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt similarity index 87% rename from localization/localization_util/CMakeLists.txt rename to localization/autoware_localization_util/CMakeLists.txt index 2e4d4b321ec54..dd18f3cbad932 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/autoware_localization_util/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(localization_util) +project(autoware_localization_util) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_util SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/util_func.cpp src/diagnostics_module.cpp src/smart_pose_buffer.cpp diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md new file mode 100644 index 0000000000000..f7fddd9eebf05 --- /dev/null +++ b/localization/autoware_localization_util/README.md @@ -0,0 +1,5 @@ +# autoware_localization_util + +`autoware_localization_util` is a localization utility package. + +This package does not have a node, it is just a library. diff --git a/localization/localization_util/include/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp similarity index 87% rename from localization/localization_util/include/localization_util/covariance_ellipse.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp index 1f187d411dc63..abd0af46856b0 100644 --- a/localization/localization_util/include/localization_util/covariance_ellipse.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ #include @@ -41,4 +41,4 @@ visualization_msgs::msg::Marker create_ellipse_marker( } // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/localization_util/include/localization_util/diagnostics_module.hpp b/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp similarity index 87% rename from localization/localization_util/include/localization_util/diagnostics_module.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp index 0ec52cfe814af..8c19c94127630 100644 --- a/localization/localization_util/include/localization_util/diagnostics_module.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ -#define LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::localization_util +{ class DiagnosticsModule { public: @@ -57,4 +59,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string template <> void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); -#endif // LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/localization_util/include/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp similarity index 75% rename from localization/localization_util/include/localization_util/matrix_type.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp index d9ec9b369127a..bda6ff19f2867 100644 --- a/localization/localization_util/include/localization_util/matrix_type.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ #include +namespace autoware::localization_util +{ using Matrix6d = Eigen::Matrix; using RowMatrixXd = Eigen::Matrix; +} // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp similarity index 86% rename from localization/localization_util/include/localization_util/smart_pose_buffer.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp index 2a2a775a9280c..8c10506c36753 100644 --- a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" #include @@ -23,6 +23,8 @@ #include +namespace autoware::localization_util +{ class SmartPoseBuffer { private: @@ -64,5 +66,6 @@ class SmartPoseBuffer const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; +} // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp similarity index 87% rename from localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp index ee25ac175c0b4..ddf7625c202ec 100644 --- a/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ /* A implementation of tree-structured parzen estimator (TPE) @@ -28,6 +28,8 @@ Optuna is also used as a reference for implementation. #include #include +namespace autoware::localization_util +{ class TreeStructuredParzenEstimator { public: @@ -80,5 +82,6 @@ class TreeStructuredParzenEstimator const std::vector sample_stddev_; Input base_stddev_; }; +} // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp similarity index 92% rename from localization/localization_util/include/localization_util/util_func.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp index 0b31333da44d3..bef9968f34b6f 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ #include #include @@ -36,6 +36,8 @@ #include #include +namespace autoware::localization_util +{ // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x); @@ -81,4 +83,6 @@ void output_pose_with_cov_to_log( const rclcpp::Logger & logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); -#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/package.xml b/localization/autoware_localization_util/package.xml similarity index 92% rename from localization/localization_util/package.xml rename to localization/autoware_localization_util/package.xml index 86c74bb92f2f8..48abf542ddd07 100644 --- a/localization/localization_util/package.xml +++ b/localization/autoware_localization_util/package.xml @@ -1,9 +1,9 @@ - localization_util + autoware_localization_util 0.1.0 - The localization_util package + The autoware_localization_util package Yamato Ando Masahiro Sakamoto Shintaro Sakoda diff --git a/localization/localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp similarity index 98% rename from localization/localization_util/src/covariance_ellipse.cpp rename to localization/autoware_localization_util/src/covariance_ellipse.cpp index 885ce2dcee19c..4a7d71909fb7a 100644 --- a/localization/localization_util/src/covariance_ellipse.cpp +++ b/localization/autoware_localization_util/src/covariance_ellipse.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/covariance_ellipse.hpp" +#include "autoware/localization_util/covariance_ellipse.hpp" #include #ifdef ROS_DISTRO_GALACTIC diff --git a/localization/localization_util/src/diagnostics_module.cpp b/localization/autoware_localization_util/src/diagnostics_module.cpp similarity index 95% rename from localization/localization_util/src/diagnostics_module.cpp rename to localization/autoware_localization_util/src/diagnostics_module.cpp index fb9e122a71e24..2b68dbf4f5890 100644 --- a/localization/localization_util/src/diagnostics_module.cpp +++ b/localization/autoware_localization_util/src/diagnostics_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" #include @@ -21,6 +21,8 @@ #include #include +namespace autoware::localization_util +{ DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { @@ -103,3 +105,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra return diagnostics_msg; } +} // namespace autoware::localization_util diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp similarity index 97% rename from localization/localization_util/src/smart_pose_buffer.cpp rename to localization/autoware_localization_util/src/smart_pose_buffer.cpp index 201c743471efd..3b529d68cf6c5 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/autoware_localization_util/src/smart_pose_buffer.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" +namespace autoware::localization_util +{ SmartPoseBuffer::SmartPoseBuffer( const rclcpp::Logger & logger, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) @@ -153,3 +155,4 @@ bool SmartPoseBuffer::validate_position_difference( } return success; } +} // namespace autoware::localization_util diff --git a/localization/localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp similarity index 97% rename from localization/localization_util/src/tree_structured_parzen_estimator.cpp rename to localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp index 8d594310d79bc..064b33ebe3ad9 100644 --- a/localization/localization_util/src/tree_structured_parzen_estimator.cpp +++ b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/tree_structured_parzen_estimator.hpp" +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" #include #include #include #include +namespace autoware::localization_util +{ // random number generator std::mt19937_64 TreeStructuredParzenEstimator::engine(0); @@ -177,3 +179,4 @@ double TreeStructuredParzenEstimator::log_gaussian_pdf( } return result; } +} // namespace autoware::localization_util diff --git a/localization/localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp similarity index 97% rename from localization/localization_util/src/util_func.cpp rename to localization/autoware_localization_util/src/util_func.cpp index 133442df68dcd..6b019f7750471 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/autoware_localization_util/src/util_func.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" -#include "localization_util/matrix_type.hpp" +#include "autoware/localization_util/matrix_type.hpp" +namespace autoware::localization_util +{ // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { @@ -249,3 +251,4 @@ void output_pose_with_cov_to_log( << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) << "," << covariance(4, 4) << "," << covariance(5, 5)); } +} // namespace autoware::localization_util diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp similarity index 88% rename from localization/localization_util/test/test_smart_pose_buffer.cpp rename to localization/autoware_localization_util/test/test_smart_pose_buffer.cpp index a8ed6d98b6064..d55555682be84 100644 --- a/localization/localization_util/test/test_smart_pose_buffer.cpp +++ b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/smart_pose_buffer.hpp" -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/util_func.hpp" #include #include @@ -26,6 +26,7 @@ #include using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; +using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; bool compare_pose( const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) @@ -56,7 +57,8 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT old_pose_ptr->pose.pose.position.x = 10.0; old_pose_ptr->pose.pose.position.y = 20.0; old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 0.0); + old_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); smart_pose_buffer.push_back(old_pose_ptr); // second data @@ -66,7 +68,8 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT new_pose_ptr->pose.pose.position.x = 20.0; new_pose_ptr->pose.pose.position.y = 40.0; new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 90.0); + new_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); smart_pose_buffer.push_back(new_pose_ptr); // interpolate @@ -90,7 +93,7 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = get_rpy(result.interpolated_pose.pose.pose); + const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); diff --git a/localization/localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp similarity index 93% rename from localization/localization_util/test/test_tpe.cpp rename to localization/autoware_localization_util/test/test_tpe.cpp index fd9afe8b2a75f..6cbe3c2a62571 100644 --- a/localization/localization_util/test/test_tpe.cpp +++ b/localization/autoware_localization_util/test/test_tpe.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/tree_structured_parzen_estimator.hpp" +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" #include +using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; + TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) { auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/autoware_ndt_scan_matcher/CMakeLists.txt similarity index 95% rename from localization/ndt_scan_matcher/CMakeLists.txt rename to localization/autoware_ndt_scan_matcher/CMakeLists.txt index b7a70b8442458..98bdf9be8761e 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/autoware_ndt_scan_matcher/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(ndt_scan_matcher) +project(autoware_ndt_scan_matcher) find_package(autoware_cmake REQUIRED) autoware_package() @@ -32,7 +32,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher" diff --git a/localization/ndt_scan_matcher/README.md b/localization/autoware_ndt_scan_matcher/README.md similarity index 97% rename from localization/ndt_scan_matcher/README.md rename to localization/autoware_ndt_scan_matcher/README.md index 1881deab23d05..22e56930a0048 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/autoware_ndt_scan_matcher/README.md @@ -1,8 +1,8 @@ -# ndt_scan_matcher +# autoware_ndt_scan_matcher ## Purpose -ndt_scan_matcher is a package for position estimation using the NDT scan matching method. +autoware_ndt_scan_matcher is a package for position estimation using the NDT scan matching method. There are two main functions in this package: @@ -58,31 +58,31 @@ One optional function is regularization. Please see the regularization chapter i #### Frame -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/frame.json") }} #### Sensor Points -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/sensor_points.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json") }} #### Ndt -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/ndt.json") }} #### Initial Pose Estimation -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} #### Validation -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/validation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/validation.json") }} #### Score Estimation -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json") }} #### Covariance -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/covariance.json") }} ## Regularization @@ -158,7 +158,7 @@ This is because if the base position is far off from the true value, NDT scan ma ### Parameters -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt_regularization.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/ndt_regularization.json") }} Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes. @@ -208,7 +208,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} ### Notes for dynamic map loading @@ -235,7 +235,7 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Parameters -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} ## 2D real-time covariance estimation @@ -262,7 +262,7 @@ initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of th initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature. -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} ## Diagnostics diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/autoware_ndt_scan_matcher/config/ndt_scan_matcher.param.yaml similarity index 100% rename from localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml rename to localization/autoware_ndt_scan_matcher/config/ndt_scan_matcher.param.yaml diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp similarity index 97% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp index 8244bb7ab4b92..c1ffbe696fda3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ -#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ #include @@ -196,4 +196,4 @@ struct HyperParameters } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp similarity index 86% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index 24ecc17ff7af1..9b140977f4971 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "localization_util/diagnostics_module.hpp" -#include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/hyper_parameters.hpp" -#include "ndt_scan_matcher/particle.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/util_func.hpp" +#include "autoware/ndt_scan_matcher/hyper_parameters.hpp" +#include "autoware/ndt_scan_matcher/particle.hpp" #include #include @@ -42,6 +42,7 @@ namespace autoware::ndt_scan_matcher { +using DiagnosticsModule = autoware::localization_util::DiagnosticsModule; class MapUpdateModule { @@ -100,4 +101,4 @@ class MapUpdateModule } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp similarity index 93% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 649a0995e0d63..c47af70f4c189 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ -#define NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ #define FMT_HEADER_ONLY -#include "localization_util/diagnostics_module.hpp" -#include "localization_util/smart_pose_buffer.hpp" -#include "ndt_scan_matcher/hyper_parameters.hpp" -#include "ndt_scan_matcher/map_update_module.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/ndt_scan_matcher/hyper_parameters.hpp" +#include "autoware/ndt_scan_matcher/map_update_module.hpp" #include #include @@ -202,13 +202,13 @@ class NDTScanMatcher : public rclcpp::Node Eigen::Matrix4f base_to_sensor_matrix_; std::mutex ndt_ptr_mtx_; - std::unique_ptr initial_pose_buffer_; + std::unique_ptr initial_pose_buffer_; // Keep latest position for dynamic map loading std::mutex latest_ekf_position_mtx_; std::optional latest_ekf_position_ = std::nullopt; - std::unique_ptr regularization_pose_buffer_; + std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; std::unique_ptr diagnostics_scan_points_; @@ -225,4 +225,4 @@ class NDTScanMatcher : public rclcpp::Node } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/particle.hpp similarity index 90% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/particle.hpp index c5ecc7f2e9837..71b5d89ceb151 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/particle.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__PARTICLE_HPP_ -#define NDT_SCAN_MATCHER__PARTICLE_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__PARTICLE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__PARTICLE_HPP_ #include #include @@ -43,4 +43,4 @@ void push_debug_markers( } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__PARTICLE_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__PARTICLE_HPP_ diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/autoware_ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml similarity index 86% rename from localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml rename to localization/autoware_ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 671e2ace56cad..2d1a5ce4c6c35 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/autoware_ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -1,5 +1,5 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/localization/ndt_scan_matcher/media/bridge_map.jpg b/localization/autoware_ndt_scan_matcher/media/bridge_map.jpg similarity index 100% rename from localization/ndt_scan_matcher/media/bridge_map.jpg rename to localization/autoware_ndt_scan_matcher/media/bridge_map.jpg diff --git a/localization/ndt_scan_matcher/media/bridge_map_less_feature.jpg b/localization/autoware_ndt_scan_matcher/media/bridge_map_less_feature.jpg similarity index 100% rename from localization/ndt_scan_matcher/media/bridge_map_less_feature.jpg rename to localization/autoware_ndt_scan_matcher/media/bridge_map_less_feature.jpg diff --git a/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png b/localization/autoware_ndt_scan_matcher/media/calculation_of_ndt_covariance.png similarity index 100% rename from localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png rename to localization/autoware_ndt_scan_matcher/media/calculation_of_ndt_covariance.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_map_update_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_map_update_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_map_update_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_map_update_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_scan_matching_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_scan_matching_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png diff --git a/localization/ndt_scan_matcher/media/differential_area_loading.gif b/localization/autoware_ndt_scan_matcher/media/differential_area_loading.gif similarity index 100% rename from localization/ndt_scan_matcher/media/differential_area_loading.gif rename to localization/autoware_ndt_scan_matcher/media/differential_area_loading.gif diff --git a/localization/ndt_scan_matcher/media/trajectory_with_regularization.png b/localization/autoware_ndt_scan_matcher/media/trajectory_with_regularization.png similarity index 100% rename from localization/ndt_scan_matcher/media/trajectory_with_regularization.png rename to localization/autoware_ndt_scan_matcher/media/trajectory_with_regularization.png diff --git a/localization/ndt_scan_matcher/media/trajectory_without_regularization.png b/localization/autoware_ndt_scan_matcher/media/trajectory_without_regularization.png similarity index 100% rename from localization/ndt_scan_matcher/media/trajectory_without_regularization.png rename to localization/autoware_ndt_scan_matcher/media/trajectory_without_regularization.png diff --git a/localization/ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml similarity index 92% rename from localization/ndt_scan_matcher/package.xml rename to localization/autoware_ndt_scan_matcher/package.xml index c88760d3db92c..a4e5d71fa5c59 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -1,9 +1,9 @@ - ndt_scan_matcher + autoware_ndt_scan_matcher 0.1.0 - The ndt_scan_matcher package + The autoware_ndt_scan_matcher package Yamato Ando Kento Yabuuchi Masahiro Sakamoto @@ -17,13 +17,13 @@ ament_cmake_auto autoware_cmake + autoware_localization_util autoware_map_msgs autoware_universe_utils diagnostic_msgs fmt geometry_msgs libpcl-all-dev - localization_util nav_msgs ndt_omp pcl_conversions diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/autoware_ndt_scan_matcher/schema/ndt_scan_matcher.schema.json similarity index 100% rename from localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json rename to localization/autoware_ndt_scan_matcher/schema/ndt_scan_matcher.schema.json diff --git a/localization/ndt_scan_matcher/schema/sub/covariance.json b/localization/autoware_ndt_scan_matcher/schema/sub/covariance.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/covariance.json rename to localization/autoware_ndt_scan_matcher/schema/sub/covariance.json diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/autoware_ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json diff --git a/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json b/localization/autoware_ndt_scan_matcher/schema/sub/dynamic_map_loading.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json rename to localization/autoware_ndt_scan_matcher/schema/sub/dynamic_map_loading.json diff --git a/localization/ndt_scan_matcher/schema/sub/frame.json b/localization/autoware_ndt_scan_matcher/schema/sub/frame.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/frame.json rename to localization/autoware_ndt_scan_matcher/schema/sub/frame.json diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json diff --git a/localization/ndt_scan_matcher/schema/sub/ndt.json b/localization/autoware_ndt_scan_matcher/schema/sub/ndt.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/ndt.json rename to localization/autoware_ndt_scan_matcher/schema/sub/ndt.json diff --git a/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json b/localization/autoware_ndt_scan_matcher/schema/sub/ndt_regularization.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/ndt_regularization.json rename to localization/autoware_ndt_scan_matcher/schema/sub/ndt_regularization.json diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation.json b/localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/score_estimation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json b/localization/autoware_ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json rename to localization/autoware_ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/sensor_points.json rename to localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/autoware_ndt_scan_matcher/schema/sub/validation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/validation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/validation.json diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp similarity index 99% rename from localization/ndt_scan_matcher/src/map_update_module.cpp rename to localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 611d15783ea38..31e0ae2eb1a59 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/map_update_module.hpp" +#include "autoware/ndt_scan_matcher/map_update_module.hpp" namespace autoware::ndt_scan_matcher { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp similarity index 96% rename from localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp rename to localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 47436832cdb5e..8cc397c4531b5 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" +#include "autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp" -#include "localization_util/matrix_type.hpp" -#include "localization_util/tree_structured_parzen_estimator.hpp" -#include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/particle.hpp" +#include "autoware/localization_util/matrix_type.hpp" +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" +#include "autoware/localization_util/util_func.hpp" +#include "autoware/ndt_scan_matcher/particle.hpp" #include #include @@ -39,6 +39,14 @@ namespace autoware::ndt_scan_matcher { +using autoware::localization_util::exchange_color_crc; +using autoware::localization_util::matrix4f_to_pose; +using autoware::localization_util::point_to_vector3d; +using autoware::localization_util::pose_to_matrix4f; + +using autoware::localization_util::DiagnosticsModule; +using autoware::localization_util::SmartPoseBuffer; +using autoware::localization_util::TreeStructuredParzenEstimator; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -593,8 +601,8 @@ bool NDTScanMatcher::callback_sensor_points_main( } // check distance_initial_to_result - const auto distance_initial_to_result = static_cast( - norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); + const auto distance_initial_to_result = static_cast(autoware::localization_util::norm( + interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result); if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) { std::stringstream message; @@ -798,18 +806,18 @@ void NDTScanMatcher::publish_initial_to_result( initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); - const auto initial_to_result_distance = - static_cast(norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); + const auto initial_to_result_distance = static_cast(autoware::localization_util::norm( + initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_pub_->publish( make_float32_stamped(sensor_ros_time, initial_to_result_distance)); - const auto initial_to_result_distance_old = - static_cast(norm(initial_pose_old_msg.pose.pose.position, result_pose_msg.position)); + const auto initial_to_result_distance_old = static_cast(autoware::localization_util::norm( + initial_pose_old_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_old_pub_->publish( make_float32_stamped(sensor_ros_time, initial_to_result_distance_old)); - const auto initial_to_result_distance_new = - static_cast(norm(initial_pose_new_msg.pose.pose.position, result_pose_msg.position)); + const auto initial_to_result_distance_new = static_cast(autoware::localization_util::norm( + initial_pose_new_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_new_pub_->publish( make_float32_stamped(sensor_ros_time, initial_to_result_distance_new)); } @@ -1010,7 +1018,8 @@ void NDTScanMatcher::service_ndt_align_main( diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", true); // transform pose_frame to map_frame - auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + auto initial_pose_msg_in_map_frame = + autoware::localization_util::transform(req->pose_with_covariance, transform_s2t); initial_pose_msg_in_map_frame.header.stamp = req->pose_with_covariance.header.stamp; map_update_module_->update_map( initial_pose_msg_in_map_frame.pose.pose.position, diagnostics_ndt_align_); @@ -1062,10 +1071,11 @@ void NDTScanMatcher::service_ndt_align_main( std::tuple NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { - output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); + autoware::localization_util::output_pose_with_cov_to_log( + get_logger(), "align_pose_input", initial_pose_with_cov); - const auto base_rpy = get_rpy(initial_pose_with_cov); - const Eigen::Map covariance = { + const auto base_rpy = autoware::localization_util::get_rpy(initial_pose_with_cov); + const Eigen::Map covariance = { initial_pose_with_cov.pose.covariance.data(), 6, 6}; const double stddev_x = std::sqrt(covariance(0, 0)); const double stddev_y = std::sqrt(covariance(1, 1)); @@ -1127,7 +1137,7 @@ std::tuple NDTScanMatcher } const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); - const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + const geometry_msgs::msg::Vector3 rpy = autoware::localization_util::get_rpy(pose); TreeStructuredParzenEstimator::Input result(6); result[0] = pose.position.x; @@ -1154,7 +1164,8 @@ std::tuple NDTScanMatcher result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; - output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); + autoware::localization_util::output_pose_with_cov_to_log( + get_logger(), "align_pose_output", result_pose_with_cov_msg); diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score); diff --git a/localization/ndt_scan_matcher/src/particle.cpp b/localization/autoware_ndt_scan_matcher/src/particle.cpp similarity index 94% rename from localization/ndt_scan_matcher/src/particle.cpp rename to localization/autoware_ndt_scan_matcher/src/particle.cpp index fce6cdce3326c..a5db875fc3ff7 100644 --- a/localization/ndt_scan_matcher/src/particle.cpp +++ b/localization/autoware_ndt_scan_matcher/src/particle.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/particle.hpp" +#include "autoware/ndt_scan_matcher/particle.hpp" -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" namespace autoware::ndt_scan_matcher { +using autoware::localization_util::exchange_color_crc; + void push_debug_markers( visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, const Particle & particle, const size_t i) diff --git a/localization/ndt_scan_matcher/test/stub_initialpose_client.hpp b/localization/autoware_ndt_scan_matcher/test/stub_initialpose_client.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_initialpose_client.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_initialpose_client.hpp diff --git a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp b/localization/autoware_ndt_scan_matcher/test/stub_pcd_loader.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_pcd_loader.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_pcd_loader.hpp diff --git a/localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp b/localization/autoware_ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp diff --git a/localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp b/localization/autoware_ndt_scan_matcher/test/stub_trigger_node_client.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_trigger_node_client.hpp diff --git a/localization/ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp b/localization/autoware_ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp similarity index 100% rename from localization/ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp rename to localization/autoware_ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp diff --git a/localization/ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp b/localization/autoware_ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp similarity index 100% rename from localization/ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp rename to localization/autoware_ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp diff --git a/localization/ndt_scan_matcher/test/test_fixture.hpp b/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp similarity index 93% rename from localization/ndt_scan_matcher/test/test_fixture.hpp rename to localization/autoware_ndt_scan_matcher/test/test_fixture.hpp index c14c0b2ffe087..36704aaa99ef2 100644 --- a/localization/ndt_scan_matcher/test/test_fixture.hpp +++ b/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp @@ -15,7 +15,7 @@ #ifndef TEST_FIXTURE_HPP_ #define TEST_FIXTURE_HPP_ -#include "../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp" +#include "../include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp" #include "stub_initialpose_client.hpp" #include "stub_pcd_loader.hpp" #include "stub_sensor_pcd_publisher.hpp" @@ -37,8 +37,9 @@ class TestNDTScanMatcher : public ::testing::Test protected: void SetUp() override { - const std::string yaml_path = ament_index_cpp::get_package_share_directory("ndt_scan_matcher") + - "/config/ndt_scan_matcher.param.yaml"; + const std::string yaml_path = + ament_index_cpp::get_package_share_directory("autoware_ndt_scan_matcher") + + "/config/ndt_scan_matcher.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { diff --git a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py b/localization/autoware_ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py similarity index 97% rename from localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py rename to localization/autoware_ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py index 2dc4958c4704f..fb182c8c51bfb 100644 --- a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py +++ b/localization/autoware_ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py @@ -33,7 +33,7 @@ @pytest.mark.launch_test def generate_test_description(): test_ndt_scan_matcher_launch_file = os.path.join( - get_package_share_directory("ndt_scan_matcher"), + get_package_share_directory("autoware_ndt_scan_matcher"), "launch", "ndt_scan_matcher.launch.xml", ) diff --git a/localization/ndt_scan_matcher/test/test_util.hpp b/localization/autoware_ndt_scan_matcher/test/test_util.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/test_util.hpp rename to localization/autoware_ndt_scan_matcher/test/test_util.hpp diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index b834bc88116a3..60ecf4bde3508 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -73,13 +73,13 @@ Here is a flowchart depicting the process and the predefined thresholds: ```mermaid graph TD - gnss_poser["gnss_poser"] --> |"/sensing/gnss/\npose_with_covariance"| pose_covariance_modifier_node - ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/\npose_with_covariance"| pose_covariance_modifier_node + gnss_poser["gnss_poser"] --> |"/sensing/gnss/
pose_with_covariance"| pose_covariance_modifier_node + ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/
pose_with_covariance"| pose_covariance_modifier_node subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"] - pc1{{"gnss_pose_yaw\nstddev"}} - pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z\nstddev"}} - pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy\nstddev"}} + pc1{{"gnss_pose_yaw
stddev"}} + pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z
stddev"}} + pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy
stddev"}} pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose") pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose @@ -117,8 +117,8 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### Without this condition (default) -- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is directly sent - to [ekf_localizer](../../localization/ekf_localizer). +- The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is directly sent + to [ekf_localizer](../../localization/autoware_ekf_localizer). - It has a preset covariance value. - **topic name:** `/localization/pose_estimator/pose_with_covariance` - The GNSS pose does not enter the ekf_localizer. @@ -126,11 +126,11 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### With this condition -- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is renamed +- The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is renamed - **from:** `/localization/pose_estimator/pose_with_covariance`. - **to:** `/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance`. - The `ndt_scan_matcher` output enters the `autoware_pose_covariance_modifier`. -- The output of this package goes to [ekf_localizer](../../localization/ekf_localizer) with: +- The output of this package goes to [ekf_localizer](../../localization/autoware_ekf_localizer) with: - **topic name:** `/localization/pose_estimator/pose_with_covariance`. ## Node diff --git a/localization/autoware_pose_covariance_modifier/package.xml b/localization/autoware_pose_covariance_modifier/package.xml index c6c5a49f991dd..dc4b741edff79 100644 --- a/localization/autoware_pose_covariance_modifier/package.xml +++ b/localization/autoware_pose_covariance_modifier/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake + autoware_interpolation geometry_msgs - interpolation rclcpp rclcpp_components std_msgs diff --git a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp index bb811c55d585d..53d6ecb244f3e 100644 --- a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp +++ b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp @@ -14,7 +14,7 @@ #include "include/pose_covariance_modifier.hpp" -#include +#include #include #include @@ -179,7 +179,7 @@ std::array PoseCovarianceModifierNode::update_ndt_covariances_from_g const double input_normalized = (x - x_min) / (x_max - x_min); // Interpolate to the output range - return interpolation::lerp(y_min, y_max, input_normalized); + return autoware::interpolation::lerp(y_min, y_max, input_normalized); }; auto ndt_variance_from_gnss_variance = [&](double ndt_variance, double gnss_variance) { diff --git a/localization/autoware_pose_estimator_arbiter/README.md b/localization/autoware_pose_estimator_arbiter/README.md index 938bb20e252ac..87fcbc252cd8e 100644 --- a/localization/autoware_pose_estimator_arbiter/README.md +++ b/localization/autoware_pose_estimator_arbiter/README.md @@ -32,7 +32,7 @@ Also, even if both can be activated at the same time, the Kalman Filter may be a ### Supporting pose_estimators -- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) +- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_ndt_scan_matcher) - [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/) - [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc) - [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_landmark_based_localizer) diff --git a/localization/autoware_pose_initializer/CMakeLists.txt b/localization/autoware_pose_initializer/CMakeLists.txt index d07858d0b704e..521b07f1d6fc9 100644 --- a/localization/autoware_pose_initializer/CMakeLists.txt +++ b/localization/autoware_pose_initializer/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/gnss_module.cpp src/localization_module.cpp src/stop_check_module.cpp + src/pose_error_check_module.cpp src/ekf_localization_trigger_module.cpp src/ndt_localization_trigger_module.cpp ) diff --git a/localization/autoware_pose_initializer/README.md b/localization/autoware_pose_initializer/README.md index e4b9bdd28b2c5..2d8f0343f3493 100644 --- a/localization/autoware_pose_initializer/README.md +++ b/localization/autoware_pose_initializer/README.md @@ -40,6 +40,7 @@ This node depends on the map height fitter library. | ------------------------------------ | ------------------------------------------------------------ | --------------------------- | | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | diagnostics | ## Diagnostics diff --git a/localization/autoware_pose_initializer/config/pose_initializer.param.yaml b/localization/autoware_pose_initializer/config/pose_initializer.param.yaml index 90ec78257e6cb..e403160fb55b5 100644 --- a/localization/autoware_pose_initializer/config/pose_initializer.param.yaml +++ b/localization/autoware_pose_initializer/config/pose_initializer.param.yaml @@ -6,6 +6,8 @@ gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + pose_error_threshold: 5.0 # [m] + pose_error_check_enabled: false # check initial pose error with gnss ekf_enabled: $(var ekf_enabled) gnss_enabled: $(var gnss_enabled) yabloc_enabled: $(var yabloc_enabled) diff --git a/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png b/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png index 7e2d8dc5e632a..cdd38242a0f24 100644 Binary files a/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png and b/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png differ diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index 67079c476ac8a..d1ffc4ca4aef5 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -19,13 +19,13 @@ ament_cmake autoware_cmake + autoware_localization_util autoware_map_height_fitter autoware_motion_utils autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs - localization_util rclcpp rclcpp_components std_srvs diff --git a/localization/autoware_pose_initializer/schema/pose_initializer.schema.json b/localization/autoware_pose_initializer/schema/pose_initializer.schema.json index d5b92c45d3038..4602223ea69e4 100644 --- a/localization/autoware_pose_initializer/schema/pose_initializer.schema.json +++ b/localization/autoware_pose_initializer/schema/pose_initializer.schema.json @@ -29,6 +29,17 @@ "default": "3.0", "minimum": 0.0 }, + "pose_error_check_enabled": { + "type": "boolean", + "description": "If true, check error between initial pose result and GNSS pose.", + "default": "false" + }, + "pose_error_threshold": { + "type": "number", + "description": "The error threshold between GNSS and initial pose", + "default": "5.0", + "minimum": 0.0 + }, "stop_check_enabled": { "type": "string", "description": "If true, initialization is accepted only when the vehicle is stopped.", @@ -75,11 +86,13 @@ "user_defined_initial_pose", "gnss_pose_timeout", "stop_check_duration", + "pose_error_threshold", "ekf_enabled", "gnss_enabled", "yabloc_enabled", "ndt_enabled", "stop_check_enabled", + "pose_error_check_enabled", "gnss_particle_covariance", "output_pose_covariance" ], diff --git a/localization/autoware_pose_initializer/src/pose_error_check_module.cpp b/localization/autoware_pose_initializer/src/pose_error_check_module.cpp new file mode 100644 index 0000000000000..b639731372d10 --- /dev/null +++ b/localization/autoware_pose_initializer/src/pose_error_check_module.cpp @@ -0,0 +1,40 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_error_check_module.hpp" + +namespace autoware::pose_initializer +{ +PoseErrorCheckModule::PoseErrorCheckModule(rclcpp::Node * node) : node_(node) +{ + pose_error_threshold_ = node_->declare_parameter("pose_error_threshold"); +} + +bool PoseErrorCheckModule::check_pose_error( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose, + double & error_2d) +{ + const double diff_pose_x = reference_pose.position.x - result_pose.position.x; + const double diff_pose_y = reference_pose.position.y - result_pose.position.y; + error_2d = std::sqrt(std::pow(diff_pose_x, 2) + std::pow(diff_pose_y, 2)); + + if (pose_error_threshold_ <= error_2d) { + RCLCPP_INFO(node_->get_logger(), "Pose Error is Large. Error is %f", error_2d); + return false; + } + + return true; +} + +} // namespace autoware::pose_initializer diff --git a/localization/autoware_pose_initializer/src/pose_error_check_module.hpp b/localization/autoware_pose_initializer/src/pose_error_check_module.hpp new file mode 100644 index 0000000000000..358156036a8ae --- /dev/null +++ b/localization/autoware_pose_initializer/src/pose_error_check_module.hpp @@ -0,0 +1,38 @@ +// Copyright 2024 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 POSE_ERROR_CHECK_MODULE_HPP_ +#define POSE_ERROR_CHECK_MODULE_HPP_ + +#include + +#include + +namespace autoware::pose_initializer +{ +class PoseErrorCheckModule +{ +public: + explicit PoseErrorCheckModule(rclcpp::Node * node); + bool check_pose_error( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose, + double & error_2d); + +private: + rclcpp::Node * node_; + double pose_error_threshold_; +}; +} // namespace autoware::pose_initializer + +#endif // POSE_ERROR_CHECK_MODULE_HPP_ diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 777f92826de73..fc9c4afab2459 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -19,6 +19,7 @@ #include "gnss_module.hpp" #include "localization_module.hpp" #include "ndt_localization_trigger_module.hpp" +#include "pose_error_check_module.hpp" #include "stop_check_module.hpp" #include @@ -39,7 +40,8 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique(this, "pose_initializer_status"); + diagnostics_pose_reliable_ = std::make_unique( + this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { ekf_localization_trigger_ = std::make_unique(this); @@ -59,6 +61,9 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } + if (declare_parameter("pose_error_check_enabled")) { + pose_error_check_ = std::make_unique(this); + } logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); @@ -166,8 +171,26 @@ void PoseInitializer::on_initialize( diagnostics_pose_reliable_->clear(); + // check pose error between gnss pose and initial pose result + if (pose_error_check_ && gnss_) { + const auto latest_gnss_pose = get_gnss_pose(); + + double gnss_error_2d; + const bool is_gnss_pose_error_small = pose_error_check_->check_pose_error( + latest_gnss_pose.pose.pose, pose.pose.pose, gnss_error_2d); + + diagnostics_pose_reliable_->add_key_value("gnss_pose_error_2d", gnss_error_2d); + diagnostics_pose_reliable_->add_key_value( + "is_gnss_pose_error_small", is_gnss_pose_error_small); + if (!is_gnss_pose_error_small) { + std::stringstream message; + message << " Large error between Initial Pose and GNSS Pose."; + diagnostics_pose_reliable_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + } // check initial pose result and publish diagnostics - diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable); + diagnostics_pose_reliable_->add_key_value("is_initial_pose_reliable", reliable); if (!reliable) { std::stringstream message; message << "Initial Pose Estimation is Unstable."; diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index e8fef885cf577..b9a6a66590b78 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -15,7 +15,7 @@ #ifndef POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER_CORE_HPP_ -#include "localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" #include #include @@ -28,6 +28,7 @@ namespace autoware::pose_initializer { +class PoseErrorCheckModule; class StopCheckModule; class LocalizationModule; class GnssModule; @@ -56,10 +57,11 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ndt_; std::unique_ptr yabloc_; std::unique_ptr stop_check_; + std::unique_ptr pose_error_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml index af8938fd74a9f..6a4e021858f7c 100644 --- a/localization/autoware_twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -17,11 +17,11 @@ ament_cmake_auto autoware_cmake + autoware_signal_processing geometry_msgs nav_msgs rclcpp rclcpp_components - signal_processing tf2 tier4_debug_msgs diff --git a/localization/autoware_twist2accel/src/twist2accel.cpp b/localization/autoware_twist2accel/src/twist2accel.cpp index 2205a9d3a674e..0f904132f6cdc 100644 --- a/localization/autoware_twist2accel/src/twist2accel.cpp +++ b/localization/autoware_twist2accel/src/twist2accel.cpp @@ -22,6 +22,8 @@ #include #include +using autoware::signal_processing::LowpassFilter1d; + namespace autoware::twist2accel { using std::placeholders::_1; diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp index 97060c080179b..d338b256fec77 100644 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -15,7 +15,7 @@ #ifndef TWIST2ACCEL_HPP_ #define TWIST2ACCEL_HPP_ -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include @@ -37,6 +37,8 @@ #include #include +using autoware::signal_processing::LowpassFilter1d; + namespace autoware::twist2accel { class Twist2Accel : public rclcpp::Node diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md deleted file mode 100644 index a7daea33e0273..0000000000000 --- a/localization/localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# localization_util - -`localization_util` is a localization utility package. - -This package does not have a node, it is just a library. diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index e8f7e6e081e43..0b02fd59b322a 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -17,8 +17,8 @@ #include "yabloc_common/ground_server/filter/moving_averaging.hpp" +#include #include -#include #include #include @@ -38,6 +38,8 @@ #include +using autoware::signal_processing::LowpassFilter1d; + namespace yabloc::ground_server { class GroundServer : public rclcpp::Node diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index ee0ef087579f7..67fe462282981 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -18,6 +18,7 @@ autoware_lanelet2_extension autoware_map_msgs + autoware_signal_processing autoware_universe_utils cv_bridge geometry_msgs @@ -26,7 +27,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing sophus std_msgs tf2_ros diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 7870ff73c5408..bc3eb80d80339 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -25,7 +25,7 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/autoware_map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. 3. **The division size along each axis should be equal.** -4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. diff --git a/perception/autoware_compare_map_segmentation/CMakeLists.txt b/perception/autoware_compare_map_segmentation/CMakeLists.txt index 664b3f3d8066e..f4b363ea42130 100644 --- a/perception/autoware_compare_map_segmentation/CMakeLists.txt +++ b/perception/autoware_compare_map_segmentation/CMakeLists.txt @@ -89,6 +89,30 @@ install( RUNTIME DESTINATION bin ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_auto_add_gtest(test_voxel_based_compare_map_filter + test/test_voxel_based_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_based_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_distance_based_compare_map_filter + test/test_distance_based_compare_map_filter.cpp + ) + target_link_libraries(test_distance_based_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_voxel_based_approximate_compare_map_filter + test/test_voxel_based_approximate_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_based_approximate_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_voxel_distance_based_compare_map_filter + test/test_voxel_distance_based_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_distance_based_compare_map_filter ${PROJECT_NAME}) + +endif() ament_auto_package( INSTALL_TO_SHARE launch diff --git a/perception/autoware_compare_map_segmentation/package.xml b/perception/autoware_compare_map_segmentation/package.xml index 32d1039f36f4e..0fc34b697e366 100644 --- a/perception/autoware_compare_map_segmentation/package.xml +++ b/perception/autoware_compare_map_segmentation/package.xml @@ -18,7 +18,9 @@ autoware_cmake autoware_map_msgs + autoware_point_types autoware_pointcloud_preprocessor + autoware_test_utils autoware_universe_utils grid_map_pcl grid_map_ros diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 8bc22f8c31184..3a04c3dc20902 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -86,7 +86,7 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); - double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); if (downsize_ratio_z_axis <= 0.0) { RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); return; diff --git a/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..3d4ee272d7eaf --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 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 "../src/distance_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/distance_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(DistanceBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp new file mode 100644 index 0000000000000..e165efa640d15 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp @@ -0,0 +1,112 @@ +// Copyright 2024 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 "../src/voxel_based_approximate_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + + "/config/voxel_based_approximate_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelBasedApproximateCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..654d7443bd8d0 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 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 "../src/voxel_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/voxel_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..4440a5eddc426 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 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 "../src/voxel_distance_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/voxel_distance_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelDistanceBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_detected_object_feature_remover/CMakeLists.txt b/perception/autoware_detected_object_feature_remover/CMakeLists.txt index 54763f8bff9ab..58384613f7d3f 100644 --- a/perception/autoware_detected_object_feature_remover/CMakeLists.txt +++ b/perception/autoware_detected_object_feature_remover/CMakeLists.txt @@ -13,6 +13,20 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE detected_object_feature_remover_node ) +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + # run node unit testings with ROS_DOMAIN_ID isolation + ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node + test/test_detected_object_feature_remover_node.cpp + ) + target_include_directories(test_${PROJECT_NAME}_node PRIVATE src) + target_link_libraries(test_${PROJECT_NAME}_node ${${PROJECT_NAME}_LIBRARIES}) + ament_target_dependencies(test_${PROJECT_NAME}_node + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/perception/autoware_detected_object_feature_remover/package.xml b/perception/autoware_detected_object_feature_remover/package.xml index a835e289cd455..8726e2bf26071 100644 --- a/perception/autoware_detected_object_feature_remover/package.xml +++ b/perception/autoware_detected_object_feature_remover/package.xml @@ -5,6 +5,8 @@ 0.1.0 The autoware_detected_object_feature_remover package Tomoya Kimura + Yoshi Ri + Kotaro Uetake Apache License 2.0 ament_cmake @@ -19,6 +21,7 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp b/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp new file mode 100644 index 0000000000000..e54151786278a --- /dev/null +++ b/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp @@ -0,0 +1,182 @@ +// Copyright 2024 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 "detected_object_feature_remover_node.hpp" + +#include +#include + +#include "tier4_perception_msgs/msg/detail/detected_object_with_feature__struct.hpp" + +#include + +namespace +{ +using autoware::detected_object_feature_remover::DetectedObjectFeatureRemover; +using autoware::test_utils::AutowareTestManager; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +/** + * @brief Return a new `autoware::test_utils::AutowareTestManager` instance as a shared pointer. + * + * @return Shared pointer of `autoware::test_utils::AutowareTestManager`. + */ +std::shared_ptr generate_test_manager() +{ + return std::make_shared(); +} + +/** + * @brief Return a new `autoware::detected_object_feature_remover::DetectedObjectFeatureRemover` + * instance as a shared pointer. + * + * @return Shared pointer of + * `autoware::detected_object_feature_remover::DetectedObjectFeatureRemover`. + */ +std::shared_ptr generate_node( + const std::string & input_topic, const std::string & output_topic) +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments( + {"--ros-args", "-r", "~/input:=" + input_topic, "-r", "~/output:=" + output_topic}); + + return std::make_shared(node_options); +} + +/** + * @brief Return a new `autoware_perception_msgs::msg::DetectedObject` instance. + * + * @return New instance of `autoware_perception_msgs::msg::DetectedObject`. + */ +DetectedObject generate_detected_object() +{ + DetectedObject output; + output.kinematics.pose_with_covariance.pose.position.x = 1.0; + output.kinematics.pose_with_covariance.pose.position.y = 2.0; + output.kinematics.pose_with_covariance.pose.position.z = 3.0; + output.kinematics.pose_with_covariance.pose.orientation.x = 0.0; + output.kinematics.pose_with_covariance.pose.orientation.y = 0.0; + output.kinematics.pose_with_covariance.pose.orientation.z = 0.0; + output.kinematics.pose_with_covariance.pose.orientation.w = 1.0; + return output; +} + +/** + * @brief Return a new `tier4_perception_msgs::msg::DetectedObjectWithFeature` instance. + * @details Return a object including a single object created by `generate_detected_object`. + * + * @return New instance of `tier4_perception_msgs::msg::DetectedObjectWithFeature`. + */ +DetectedObjectWithFeature generate_feature_object() +{ + DetectedObjectWithFeature output; + output.object = generate_detected_object(); + return output; +} + +/** + * @brief Return a new `tier4_perception_msgs::msg::DetectedObjectsWithFeature` instance. + * @details If `as_emtpy=true`, returns a instance without any objects. Otherwise, returns a + * instance including a single object created by `generate_feature_object`. + * + * @param stamp Timestamp of the message. + * @param as_empty Whether to return a instance without any objects. + * @return New instance of `tier4_perception_mgs::msg::DetectedObjectsWIthFeature`. + */ +DetectedObjectsWithFeature generate_feature_objects(const rclcpp::Time & stamp, bool as_empty) +{ + DetectedObjectsWithFeature output; + output.header.frame_id = "base_link"; + output.header.stamp = stamp; + if (!as_empty) { + auto object = generate_feature_object(); + output.feature_objects.emplace_back(object); + } + return output; +} +} // namespace + +/** + * Test suite of DetectedObjectFeatureRemover. + * + * This test case checks whether the node works if the arbitrary object is input. + */ +TEST(FeatureRemoverTest, TestArbitraryObject) +{ + const std::string input_topic = "/detected_object_feature_remover/input"; + const std::string output_topic = "/detected_object_feature_remover/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + DetectedObjects output; + auto callback = [&output](const DetectedObjects::ConstSharedPtr msg) { + output.header = msg->header; + output.objects = msg->objects; + }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = false; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // Check output + auto expect = generate_detected_object(); + EXPECT_EQ(1, output.objects.size()); + EXPECT_EQ(expect, output.objects.front()); +} + +/** + * Test suite of DetectedObjectFeatureRemover. + * + * This test case checks whether the node works if the empty object is input. + */ +TEST(FeatureRemoverTest, TestEmptyObject) +{ + const std::string input_topic = "/detected_object_feature_remover/input"; + const std::string output_topic = "/detected_object_feature_remover/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + DetectedObjects output; + auto callback = [&output](const DetectedObjects::ConstSharedPtr msg) { + output.header = msg->header; + output.objects = msg->objects; + }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = true; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // Check output + EXPECT_EQ(0, output.objects.size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 060b7f292c403..0a69959aecf5c 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -166,7 +166,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( int index = -1; bool associated = false; double max_iou = 0.0; - bool is_roi_label_known = + const bool is_roi_label_known = feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { double iou(0.0); @@ -196,34 +196,18 @@ void RoiClusterFusionNode::fuseOnSingleImage( } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_existence_prob_higher = - output_cluster_msg.feature_objects.at(index).object.existence_probability <= - feature_obj.object.existence_probability; - if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; - - // Update existence_probability for fused objects - if ( - output_cluster_msg.feature_objects.at(index).object.existence_probability < - min_roi_existence_prob_) { - output_cluster_msg.feature_objects.at(index).object.existence_probability = - min_roi_existence_prob_; - } - } - - // fuse with unknown roi - - if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; + auto & fused_object = output_cluster_msg.feature_objects.at(index).object; + const bool is_roi_existence_prob_higher = + fused_object.existence_probability <= feature_obj.object.existence_probability; + const bool is_roi_iou_over_threshold = + (is_roi_label_known && iou_threshold_ < max_iou) || + (!is_roi_label_known && unknown_iou_threshold_ < max_iou); + + if (is_roi_iou_over_threshold && is_roi_existence_prob_higher) { + fused_object.classification = feature_obj.object.classification; // Update existence_probability for fused objects - if ( - output_cluster_msg.feature_objects.at(index).object.existence_probability < - min_roi_existence_prob_) { - output_cluster_msg.feature_objects.at(index).object.existence_probability = - min_roi_existence_prob_; - } + fused_object.existence_probability = + std::clamp(feature_obj.object.existence_probability, min_roi_existence_prob_, 1.0f); } } if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp index de901b06e83c3..9a17cad91efb8 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp @@ -35,7 +35,7 @@ uint8_t getSemanticType(const std::string & class_name); std::array convertPoseCovarianceMatrix(const Box3D & box3d); -std::array convertTwistCovarianceMatrix(const Box3D & box3d); +std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw); bool isCarLikeVehicleLabel(const uint8_t label); diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index d7fc0aef204be..feeab969e88bd 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -50,7 +50,7 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; + const float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = @@ -67,14 +67,18 @@ void box3DToDetectedObject( if (has_twist) { float vel_x = box3d.vel_x; float vel_y = box3d.vel_y; + + // twist of the object is based on the object coordinate system geometry_msgs::msg::Twist twist; - twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); - twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); + twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y; + twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + twist.angular.z = 0; // angular velocity is not supported + obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; if (has_variance) { obj.kinematics.has_twist_covariance = has_variance; - obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); + obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d, yaw); } } } @@ -111,12 +115,21 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) return pose_covariance; } -std::array convertTwistCovarianceMatrix(const Box3D & box3d) +std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw) { using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // twist covariance matrix is based on the object coordinate system std::array twist_covariance{}; - twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; - twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; + const float cos_yaw = std::cos(yaw); + const float sin_yaw = std::sin(yaw); + twist_covariance[POSE_IDX::X_X] = + box3d.vel_x_variance * cos_yaw * cos_yaw + box3d.vel_y_variance * sin_yaw * sin_yaw; + twist_covariance[POSE_IDX::X_Y] = + (box3d.vel_y_variance - box3d.vel_x_variance) * sin_yaw * cos_yaw; + twist_covariance[POSE_IDX::Y_X] = twist_covariance[POSE_IDX::X_Y]; + twist_covariance[POSE_IDX::Y_Y] = + box3d.vel_x_variance * sin_yaw * sin_yaw + box3d.vel_y_variance * cos_yaw * cos_yaw; return twist_covariance; } diff --git a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp index 555d820ec3644..5605d2df6a9d9 100644 --- a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp @@ -125,13 +125,14 @@ TEST(TestSuite, convertPoseCovarianceMatrix) TEST(TestSuite, convertTwistCovarianceMatrix) { autoware::lidar_centerpoint::Box3D box3d; - box3d.vel_x_variance = 0.1; + box3d.vel_x_variance = 0.5; box3d.vel_y_variance = 0.2; + float yaw = 0; std::array twist_covariance = - autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d); + autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d, yaw); - EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); + EXPECT_FLOAT_EQ(twist_covariance[0], 0.5); EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); } diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 9e60073de4be6..cd0b42f030eaf 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -14,12 +14,12 @@ ament_cmake autoware_cmake + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_universe_utils glog - interpolation rclcpp rclcpp_components tf2 diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 6427dd9c8914c..5a43a8fb25633 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,6 +14,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include #include #include #include @@ -24,7 +25,6 @@ #include #include #include -#include #include diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index d8f6f85b239a7..a2a1b8b3d3fda 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,9 +14,10 @@ #include "map_based_prediction/path_generator.hpp" +#include +#include #include #include -#include #include @@ -249,6 +250,15 @@ PredictedPath PathGenerator::generatePolynomialPath( terminal_point.d_vel = 0.0; terminal_point.d_acc = 0.0; + // if the object is behind of the reference path adjust the lateral_duration to reach the start of + // the reference path + double lateral_duration_adjusted = lateral_duration; + if (current_point.s < 0.0) { + const double distance_to_start = -current_point.s; + const double duration_to_reach = distance_to_start / terminal_point.s_vel; + lateral_duration_adjusted = std::max(lateral_duration, duration_to_reach); + } + // calculate terminal d position, based on backlash width { if (backlash_width < 0.01 /*m*/) { @@ -258,7 +268,7 @@ PredictedPath PathGenerator::generatePolynomialPath( } else { const double return_width = path_width / 2.0; // [m] const double current_momentum_d = - current_point.d + 0.5 * current_point.d_vel * lateral_duration; + current_point.d + 0.5 * current_point.d_vel * lateral_duration_adjusted; const double momentum_d_abs = std::abs(current_momentum_d); if (momentum_d_abs < backlash_width) { @@ -281,8 +291,8 @@ PredictedPath PathGenerator::generatePolynomialPath( } // Step 2. Generate Predicted Path on a Frenet coordinate - const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); + const auto frenet_predicted_path = generateFrenetPath( + current_point, terminal_point, ref_path_len, duration, lateral_duration_adjusted); // Step 3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 2e325d18579a6..215e55cb4ac62 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -231,7 +231,7 @@ bool BicycleMotionModel::updateStatePoseHeadVel( // update state Eigen::MatrixXd Y(DIM_Y, 1); - Y << x, y, yaw, vel; + Y << x, y, fixed_yaw, vel; Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); C(0, IDX::X) = 1.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index b1c5a36f9ad5b..bf5950bdd4023 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -194,7 +194,7 @@ bool CTRVMotionModel::updateStatePoseHeadVel( // update state Eigen::MatrixXd Y(DIM_Y, 1); - Y << x, y, yaw, vel; + Y << x, y, fixed_yaw, vel; Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); C(0, IDX::X) = 1.0; diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt index 56f14ad9d2b48..6cf93599f9964 100644 --- a/perception/autoware_shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -106,15 +106,15 @@ ament_auto_package(INSTALL_TO_SHARE ## Tests if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + set(ament_cmake_uncrustify_FOUND TRUE) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE test_files test/*.cpp test/**/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) - - target_link_libraries(test_shape_estimation - ${PROJECT_NAME} + target_include_directories(test_${PROJECT_NAME} PRIVATE src) + target_link_libraries(test_${PROJECT_NAME} ${${PROJECT_NAME}_LIBRARIES}) + ament_target_dependencies(test_${PROJECT_NAME} + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} ) endif() diff --git a/perception/autoware_shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml index fec1009428113..644a9de661eca 100644 --- a/perception/autoware_shape_estimation/package.xml +++ b/perception/autoware_shape_estimation/package.xml @@ -35,6 +35,7 @@ ament_cmake_gtest ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp b/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp new file mode 100644 index 0000000000000..df04ade05b1ce --- /dev/null +++ b/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp @@ -0,0 +1,194 @@ +// Copyright 2024 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 "shape_estimation_node.hpp" + +#include +#include + +#include "tier4_perception_msgs/msg/detail/detected_objects_with_feature__struct.hpp" + +#include + +namespace +{ +using autoware::shape_estimation::ShapeEstimationNode; +using autoware::test_utils::AutowareTestManager; +using sensor_msgs::msg::PointCloud2; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +/** + * @brief Return a new `autoware::test_utils::AutowareTestManager` instance as a shared pointer. + * + * @return Shared pointer of `autoware::test_utils::AutowareTestManager`. + */ +std::shared_ptr generate_test_manager() +{ + return std::make_shared(); +} + +/** + * @brief Return a new `autoware::shape_estimation::ShapeEstimationNode` instance as a shared + * pointer. + * + * @param input_topic Input topic name. + * @param output_topic Output topic name. + * @return Shared pointer of `autoware::shape_estimation::ShapeEstimationNode`. + */ +std::shared_ptr generate_node( + const std::string & input_topic, const std::string & output_topic) +{ + const auto package_dir = + ament_index_cpp::get_package_share_directory("autoware_shape_estimation"); + + auto node_options = rclcpp::NodeOptions(); + node_options.arguments( + {"--ros-args", "--params-file", package_dir + "/config/shape_estimation.param.yaml", "-r", + "input:=" + input_topic, "-r", "objects:=" + output_topic}); + return std::make_shared(node_options); +} + +/** + * @brief Return a new `PointCloud` object shaping like L. + * + * @param length Length of L box in [m]. + * @param width Width of L box in [m]. + * @param height Height of L box in [m]. + * @param yaw Yaw rotation offset in [rad]. Defaults to 0.0. + * @param offset_x X position offset in [m]. Defaults to 0.0. + * @param offset_y Y position offset in [m]. Defaults to 0.0. + * @return Created a `PointCloud` object. + * + * @note This function is deprecated with `test/shape_estimation/test_shape_estimator.cpp`. + */ +PointCloud2 generate_cluster( + double length, double width, double height, double yaw = 0.0, double offset_x = 0.0, + double offset_y = 0.0) +{ + constexpr double x_step = 0.4; + constexpr double y_step = 0.2; + + pcl::PointCloud cluster; + for (double x = -0.5 * length; x < 0.5 * length; x += x_step) { + cluster.emplace_back(x, 0.5 * width, 0.0); + } + for (double y = -0.5 * width; y < 0.5 * width; y += y_step) { + cluster.emplace_back(-0.5 * length, y, 0.0); + } + cluster.emplace_back(0.5 * length, -0.5 * width, 0.0); + cluster.emplace_back(0.5 * length, 0.5 * width, 0.0); + cluster.emplace_back(-0.5 * length, -0.5 * width, 0.0); + cluster.emplace_back(-0.5 * length, 0.5 * width, 0.0); + cluster.emplace_back(0.0, 0.0, height); + + if (yaw != 0.0 || offset_x != 0.0 || offset_y != 0.0) { + for (auto & point : cluster) { + const double x = point.x; + const double y = point.y; + // rotate + point.x = x * cos(yaw) - y * sin(yaw); + point.y = x * sin(yaw) + y * cos(yaw); + // offset + point.x += offset_x; + point.y += offset_y; + } + } + + PointCloud2 output; + pcl::toROSMsg(cluster, output); + return output; +} + +/** + * @brief Return a new `tier4_perception_msgs::msg::DetectedObjectsWithFeature` instance. + * + * @param stamp ROS timestamp. + * @param as_empty Whether to return a object without any cluster. + * @return `tier4_perception_msgs::msg::DetectedObjectsWithFeature`. + */ +DetectedObjectsWithFeature generate_feature_objects(const rclcpp::Time & stamp, bool as_empty) +{ + constexpr double length = 4.0; + constexpr double width = 2.0; + constexpr double height = 1.0; + + DetectedObjectsWithFeature output; + output.header.frame_id = "base_link"; + output.header.stamp = stamp; + + if (!as_empty) { + DetectedObjectWithFeature object; + object.feature.cluster = generate_cluster(length, width, height); + output.feature_objects.emplace_back(object); + } + + return output; +} +} // namespace + +/** + * Test `ShapeEstimationNode`. + * + * This test case checks whether the node works with the arbitrary input. + */ +TEST(ShapeEstimationNode, testShapeEstimationNodeWithArbitraryCluster) +{ + const std::string input_topic = "/shape_estimation/input"; + const std::string output_topic = "/shape_estimation/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + int counter = 0; + auto callback = [&counter](const DetectedObjectsWithFeature::ConstSharedPtr) { ++counter; }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = false; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // check if the node worked + EXPECT_EQ(1, counter); +} + +/** + * Test `ShapeEstimationNode`. + * + * This test case checks whether the node works with the empty cluster input. + */ +TEST(ShapeEstimationNode, testShapeEstimationNodeWithEmptyCluster) +{ + const std::string input_topic = "/shape_estimation/input"; + const std::string output_topic = "/shape_estimation/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + int counter = 0; + auto callback = [&counter](const DetectedObjectsWithFeature::ConstSharedPtr) { ++counter; }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = true; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // check if the node worked + EXPECT_EQ(1, counter); +} diff --git a/perception/perception_utils/CMakeLists.txt b/perception/perception_utils/CMakeLists.txt index 18ef18303fb1b..e4b581928cc5e 100644 --- a/perception/perception_utils/CMakeLists.txt +++ b/perception/perception_utils/CMakeLists.txt @@ -13,4 +13,10 @@ target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ) +if(BUILD_TESTING) + ament_auto_add_gtest(test_utils + test/test_utils.cpp + ) +endif() + ament_auto_package() diff --git a/perception/perception_utils/package.xml b/perception/perception_utils/package.xml index b38226991eb66..94401cc68a13e 100644 --- a/perception/perception_utils/package.xml +++ b/perception/perception_utils/package.xml @@ -14,6 +14,8 @@ libopencv-dev rclcpp + ament_cmake_gtest + ament_cmake diff --git a/perception/perception_utils/test/test_utils.cpp b/perception/perception_utils/test/test_utils.cpp new file mode 100644 index 0000000000000..d28b8489971e0 --- /dev/null +++ b/perception/perception_utils/test/test_utils.cpp @@ -0,0 +1,77 @@ +// Copyright 2024 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 "perception_utils/run_length_encoder.hpp" + +#include + +#include + +// Test case 1: Test if the decoded image is the same as the original image +TEST(UtilsTest, runLengthEncoderDecoderTest) +{ + int height = 10; + int width = 20; + uint8_t number_cls = 16; + // Create an image as below + // 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 2 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 3 3 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 4 4 4 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 5 5 5 5 5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 6 6 6 6 6 6 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 7 7 7 7 7 7 7 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 8 8 8 8 8 8 8 8 0 0 0 0 0 0 0 0 0 0 0 0 + // 9 9 9 9 9 9 9 9 9 0 0 0 0 0 0 0 0 0 0 0 + cv::Mat image = cv::Mat::zeros(10, 20, CV_8UC1); + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + if (j < i) { + image.at(i, j) = i % number_cls; + } else { + image.at(i, j) = 0; + } + } + } + // Compress the image + std::vector> compressed_data = perception_utils::runLengthEncoder(image); + int step = sizeof(uint8_t) + sizeof(int); + std::vector data(compressed_data.size() * step); + for (size_t i = 0; i < compressed_data.size(); ++i) { + std::memcpy(&data[i * step], &compressed_data.at(i).first, sizeof(uint8_t)); + std::memcpy(&data[i * step + sizeof(uint8_t)], &compressed_data.at(i).second, sizeof(int)); + } + // Decompress the image + cv::Mat decoded_image = perception_utils::runLengthDecoder(data, height, width); + // Compare the original image and the decoded image + ASSERT_EQ(image.rows, decoded_image.rows); + ASSERT_EQ(image.cols, decoded_image.cols); + bool image_eq = true; + for (int i = 0; i < image.rows; ++i) { + for (int j = 0; j < image.cols; ++j) { + if (image.at(i, j) != decoded_image.at(i, j)) { + image_eq = false; + break; + } + } + } + EXPECT_EQ(image_eq, true); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 29bd91f5c5b72..35cbf31239272 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -19,10 +19,10 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" #include #include diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index c048c3278e2ec..d063d08d82d92 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -18,7 +18,7 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" #include "autoware/obstacle_cruise_planner/planner_interface.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -26,6 +26,8 @@ #include #include +using autoware::signal_processing::LowpassFilter1d; + class PIDBasedPlanner : public PlannerInterface { public: diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 79335dc23221b..9999be9937111 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,21 +18,21 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs object_recognition_utils osqp_interface rclcpp rclcpp_components - signal_processing std_msgs tf2 tf2_ros diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 899cc66cb4331..3a24cfa7dcfe0 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -14,6 +14,9 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" @@ -21,9 +24,6 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -237,7 +237,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( } // resample optimum velocity for original each position auto resampled_opt_velocity = - interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); for (size_t i = break_id; i < stop_traj_points.size(); ++i) { resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps); } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index a021e87e6199e..fefe7fd06007e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,15 +14,17 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "interpolation/spline_interpolation.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" +using autoware::signal_processing::LowpassFilter1d; + namespace { VelocityLimit createVelocityLimitMsg( @@ -607,7 +609,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( if (unique_s_vec.back() < sum_dist) { return unique_v_vec.back(); } - return interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); }(); acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp( diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 0b9a9eaa9d58c..ba93efc1f10c4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -19,8 +19,8 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" #include #include @@ -620,7 +620,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // calculate slow down velocity const double stable_slow_down_vel = [&]() { if (prev_output) { - return signal_processing::lowpassFilter( + return autoware::signal_processing::lowpassFilter( feasible_slow_down_vel, prev_output->target_vel, slow_down_param_.lpf_gain_slow_down_vel); } return feasible_slow_down_vel; @@ -706,7 +706,7 @@ double PlannerInterface::calculateSlowDownVelocity( slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving); const double stable_precise_lat_dist = [&]() { if (prev_output) { - return signal_processing::lowpassFilter( + return autoware::signal_processing::lowpassFilter( obstacle.precise_lat_dist, prev_output->precise_lat_dist, slow_down_param_.lpf_gain_lat_dist); } @@ -785,7 +785,7 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); const double prev_dist_to_slow_down = autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); - return signal_processing::lowpassFilter( + return autoware::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; diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index 71eec208f7fdc..9e6a8a2dc5f43 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -25,6 +25,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs @@ -34,7 +35,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing std_msgs tf2 tf2_eigen diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3443ab663b642..3d9192c93d5b1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ #define AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" @@ -22,8 +24,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "osqp_interface/osqp_interface.hpp" #include @@ -45,9 +45,9 @@ struct Bounds static Bounds lerp(Bounds prev_bounds, Bounds next_bounds, double ratio) { const double lower_bound = - interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); + autoware::interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); const double upper_bound = - interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); + autoware::interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); return Bounds{lower_bound, upper_bound}; } @@ -249,10 +249,10 @@ class MPTOptimizer const PlannerData & planner_data, const std::vector & smoothed_points) const; void updateCurvature( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateOrientation( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateBounds( std::vector & ref_points, const std::vector & left_bound, @@ -266,7 +266,7 @@ class MPTOptimizer const double ego_vel) const; void updateVehicleBounds( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateFixedPoint(std::vector & ref_points) const; void updateDeltaArcLength(std::vector & ref_points) const; void updateExtraPoints(std::vector & ref_points) const; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp index edc91bd40d4dc..48e7d68e70e18 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index c9900d2ce8225..ff53cfbc3c230 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 0830d534c7cb0..1a7869b6a87fd 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,13 +14,13 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 367fc915badba..fdffc8a926c24 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -14,13 +14,13 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tf2/utils.h" #include @@ -568,7 +568,7 @@ std::vector MPTOptimizer::calcReferencePoints( // remove repeated points ref_points = trajectory_utils::sanitizePoints(ref_points); - SplineInterpolationPoints2d ref_points_spline(ref_points); + autoware::interpolation::SplineInterpolationPoints2d ref_points_spline(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); // 3. calculate orientation and curvature @@ -580,7 +580,7 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points = autoware::motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length); - ref_points_spline = SplineInterpolationPoints2d(ref_points); + ref_points_spline = autoware::interpolation::SplineInterpolationPoints2d(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); // 5. update fixed points, and resample @@ -588,7 +588,7 @@ std::vector MPTOptimizer::calcReferencePoints( // New start point may be added and resampled. Spline calculation is required. updateFixedPoint(ref_points); ref_points = trajectory_utils::sanitizePoints(ref_points); - ref_points_spline = SplineInterpolationPoints2d(ref_points); + ref_points_spline = autoware::interpolation::SplineInterpolationPoints2d(ref_points); // 6. update bounds // NOTE: After this, resample must not be called since bounds are not interpolated. @@ -614,7 +614,7 @@ std::vector MPTOptimizer::calcReferencePoints( void MPTOptimizer::updateOrientation( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { @@ -625,7 +625,7 @@ void MPTOptimizer::updateOrientation( void MPTOptimizer::updateCurvature( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { const auto curvature_vec = ref_points_spline.getSplineInterpolatedCurvatures(); for (size_t i = 0; i < ref_points.size(); ++i) { @@ -1092,7 +1092,7 @@ void MPTOptimizer::avoidSuddenSteering( void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1181,10 +1181,10 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( } // for avoidance if (0 < ref_points.at(i).normalized_avoidance_cost) { - const double lat_error_weight = interpolation::lerp( + const double lat_error_weight = autoware::interpolation::lerp( mpt_param_.lat_error_weight, mpt_param_.avoidance_lat_error_weight, ref_points.at(i).normalized_avoidance_cost); - const double yaw_error_weight = interpolation::lerp( + const double yaw_error_weight = autoware::interpolation::lerp( mpt_param_.yaw_error_weight, mpt_param_.avoidance_yaw_error_weight, ref_points.at(i).normalized_avoidance_cost); return {lat_error_weight, yaw_error_weight}; @@ -1206,7 +1206,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( // update R std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { - const double adaptive_steer_weight = interpolation::lerp( + const double adaptive_steer_weight = autoware::interpolation::lerp( mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, ref_points.at(i).normalized_avoidance_cost); R_triplet_vec.push_back(Eigen::Triplet(D_u * i, D_u * i, adaptive_steer_weight)); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index f0d101a046206..62a4882f8a4c1 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -14,12 +14,12 @@ #include "autoware/path_optimizer/node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_optimizer/debug_marker.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index bcfc7e53ee67d..f5ef2ddcd2451 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -181,7 +181,7 @@ std::vector resampleReferencePoints( query_keys.push_back(base_keys.back() - epsilon); } - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); // create output reference points by updating curvature with resampled one std::vector output_ref_points; @@ -200,7 +200,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::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)) { diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index f989ab76f6952..e7f64a896bdd3 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 6cecef433fc3e..b9b79bb6ceaf6 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -14,12 +14,12 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs - interpolation nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index b92798f92728c..a3082a2c979c3 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -14,10 +14,10 @@ #include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index e533b963cf655..37c27eaae6f93 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -79,7 +79,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::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)) { diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 13e13b642af1f..6202be28e7e87 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -326,6 +326,9 @@ class RouteHandler */ lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; + Pose get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const; + private: // MUST lanelet::routing::RoutingGraphPtr routing_graph_ptr_; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0c0d14f95c768..616b91d1bc5f9 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -47,6 +47,8 @@ namespace autoware::route_handler { namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; @@ -2035,4 +2037,29 @@ std::optional RouteHandler::findDrivableLanePath( if (route) return route->shortestPath(); return {}; } + +Pose RouteHandler::get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const +{ + double accumulated_distance2d = 0; + for (const auto & llt : lanelet_sequence) { + const auto & centerline = llt.centerline(); + for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) { + const auto pt = *it; + const auto next_pt = *std::next(it); + const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt)); + if (accumulated_distance2d + distance2d > s) { + const double ratio = (s - accumulated_distance2d) / distance2d; + const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio; + const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x()); + Pose pose; + pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + pose.orientation = createQuaternionFromYaw(yaw); + return pose; + } + accumulated_distance2d += distance2d; + } + } + return Pose{}; +} } // namespace autoware::route_handler diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 931b815b176c3..2abae4af2ca67 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -17,6 +17,7 @@ autoware_behavior_path_planner_common autoware_geography_utils + autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_map_projection_loader @@ -31,7 +32,6 @@ autoware_vehicle_info_utils geometry_msgs global_parameter_loader - interpolation map_loader osqp_interface rclcpp diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 256cd2386c431..a6ba007e71485 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,7 @@ #include "static_centerline_generator_node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" #include "autoware/map_projection_loader/map_projection_loader.hpp" #include "autoware/motion_utils/resample/resample.hpp" @@ -25,7 +26,6 @@ #include "autoware_lanelet2_extension/utility/utilities.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "type_alias.hpp" #include "utils.hpp" @@ -625,7 +625,7 @@ void StaticCenterlineGeneratorNode::validate() } // calculate curvature - SplineInterpolationPoints2d centerline_spline(centerline); + autoware::interpolation::SplineInterpolationPoints2d centerline_spline(centerline); const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures(); const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle( vehicle_info_.max_steer_angle_rad - max_steer_angle_margin); diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index f5cd3f2667c04..65684e414e90d 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -21,13 +21,13 @@ autoware_cmake eigen3_cmake_module + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs osqp_interface qp_interface diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 6e12d4a6f20fb..48f3138cfe151 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -14,7 +14,7 @@ #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -234,8 +234,10 @@ bool calcStopVelocityWithConstantJerkAccLimit( } if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distances) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distances)) { + !autoware::interpolation::isIncreasing(xs) || + !autoware::interpolation::isIncreasing(distances) || + !autoware::interpolation::isNotDecreasing(xs) || + !autoware::interpolation::isNotDecreasing(distances)) { return false; } @@ -245,9 +247,9 @@ bool calcStopVelocityWithConstantJerkAccLimit( return false; } - const auto vel_at_wp = interpolation::lerp(xs, vs, distances); - const auto acc_at_wp = interpolation::lerp(xs, as, distances); - const auto jerk_at_wp = interpolation::lerp(xs, js, distances); + const auto vel_at_wp = autoware::interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = autoware::interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = autoware::interpolation::lerp(xs, js, distances); for (size_t i = 0; i < vel_at_wp.size(); ++i) { output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 68f05438d38c8..cf031601cfd57 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/linear_interpolation.hpp" #include #include @@ -88,10 +88,10 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); - traj_p.longitudinal_velocity_mps = interpolation::lerp( + traj_p.longitudinal_velocity_mps = autoware::interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = - interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); + autoware::interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); } return traj_p; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index 384ef7bd285e9..f6974e38a2a50 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -7,7 +7,6 @@ # avoidance is performed for the object type with true target_object: car: - execute_num: 2 # [-] th_moving_speed: 1.0 # [m/s] th_moving_time: 1.0 # [s] max_expand_ratio: 0.0 # [-] @@ -17,7 +16,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] truck: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -27,7 +25,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] bus: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -37,7 +34,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] trailer: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -47,7 +43,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] unknown: - execute_num: 1 th_moving_speed: 0.28 # 1.0km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -57,7 +52,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: - execute_num: 2 th_moving_speed: 0.28 # 1.0km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -67,7 +61,6 @@ hard_margin: 1.0 # [m] hard_margin_for_parked_vehicle: 1.0 # [m] motorcycle: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -77,7 +70,6 @@ hard_margin: 1.0 # [m] hard_margin_for_parked_vehicle: 1.0 # [m] pedestrian: - execute_num: 2 th_moving_speed: 0.28 # 1.0km/h th_moving_time: 1.0 max_expand_ratio: 0.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index fffb86767b0a8..1051a3460be3b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -49,6 +49,7 @@ bool AvoidanceByLaneChangeInterface::isExecutionRequested() const return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } + void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ddcfda50d18c4..28ff6c9e110d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -66,7 +66,6 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); @@ -187,8 +186,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 95609a430ac80..44f842a67c236 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::SceneModuleInterface; +using SMIPtr = std::unique_ptr; + class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: @@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override; + SMIPtr createNewSceneModuleInstance() override; private: std::shared_ptr avoidance_parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 25a155c324a55..991b5c8af656d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -34,13 +34,55 @@ #include #include +namespace +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon create_execution_area( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset) +{ + 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; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} +} // namespace + namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::Direction; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::ObjectInfo; using autoware::behavior_path_planner::Point2d; -using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea; AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, @@ -82,9 +124,9 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto & nearest_object = data.target_objects.front(); const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); - const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); + const auto minimum_lane_change_length = calc_minimum_dist_buffer(); - lane_change_debug_.execution_area = createExecutionArea( + lane_change_debug_.execution_area = create_execution_area( getCommonParam().vehicle_info, getEgoPose(), std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); @@ -273,16 +315,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ return avoidance_helper_->getMinAvoidanceDistance(shift_length); } -double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +double AvoidanceByLaneChange::calc_minimum_dist_buffer() const { - const auto current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "no empty lanes"); - return std::numeric_limits::infinity(); - } - - return utils::lane_change::calculation::calc_minimum_lane_change_length( - getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); + const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer( + common_data_ptr_, get_current_lanes()); + return dist_buffer.min; } double AvoidanceByLaneChange::calcLateralOffset() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp index d6bf6bc29df97..42ae41fa380cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; double calcMinAvoidanceLength(const ObjectData & nearest_object) const; - double calcMinimumLaneChangeLength() const; + double calc_minimum_dist_buffer() const; double calcLateralOffset() const; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 5bf00292d061e..9308aab7642f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -22,6 +22,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_signal_processing autoware_universe_utils autoware_vehicle_msgs geometry_msgs @@ -29,7 +30,6 @@ object_recognition_utils pluginlib rclcpp - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index d91b3fb9aa346..3fa3f04435e87 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index 626b47b6bdb0d..d3f7f7a4015f0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -10,6 +10,7 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED + src/pull_over_planner/pull_over_planner_base.cpp src/pull_over_planner/freespace_pull_over.cpp src/pull_over_planner/geometric_pull_over.cpp src/pull_over_planner/shift_pull_over.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp index a51b2fd337512..7e9ccea9ac0c2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp @@ -265,7 +265,7 @@ int main(int argc, char ** argv) auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( *node, goal_planner_parameter, lane_departure_checker); const auto pull_over_path_opt = - shift_pull_over_planner.plan(planner_data, reference_path, route_msg.goal_pose); + shift_pull_over_planner.plan(0, 0, planner_data, reference_path, route_msg.goal_pose); pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); @@ -282,7 +282,7 @@ int main(int argc, char ** argv) std::cout << pull_over_path_opt.has_value() << std::endl; if (pull_over_path_opt) { const auto & pull_over_path = pull_over_path_opt.value(); - const auto full_path = pull_over_path.getFullPath(); + const auto & full_path = pull_over_path.full_path; plot_path_with_lane_id(ax, full_path); } ax.set_aspect(Args("equal")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp new file mode 100644 index 0000000000000..67aa41a5af7e5 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 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 AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ + +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include +#include + +namespace autoware::behavior_path_planner +{ + +class PathDecisionState +{ +public: + enum class DecisionKind { + NOT_DECIDED, + DECIDING, + DECIDED, + }; + + DecisionKind state{DecisionKind::NOT_DECIDED}; + rclcpp::Time stamp{}; + bool is_stable_safe{false}; + std::optional safe_start_time{std::nullopt}; +}; + +class PathDecisionStateController +{ +public: + PathDecisionStateController() = default; + + /** + * @brief update current state and save old current state to prev state + */ + void transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const autoware_perception_msgs::msg::PredictedObjects & static_target_objects, + const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded); + + PathDecisionState get_current_state() const { return current_state_; } + + PathDecisionState get_prev_state() const { return prev_state_; } + +private: + PathDecisionState current_state_{}; + PathDecisionState prev_state_{}; + + /** + * @brief update current state and save old current state to prev state + */ + PathDecisionState get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const; +}; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index fe859b458c7bb..d029b2f5953a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -15,24 +15,15 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ -#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/thread_data.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include -#include #include -#include -#include #include #include @@ -41,7 +32,6 @@ #include #include -#include #include #include #include @@ -61,8 +51,6 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; using autoware::freespace_planning_algorithms::AstarParam; using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::PlannerCommonParam; -using autoware::freespace_planning_algorithms::RRTStar; -using autoware::freespace_planning_algorithms::RRTStarParam; using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -71,198 +59,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using autoware::universe_utils::Polygon2d; -#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) - -enum class DecidingPathStatus { - NOT_DECIDED, - DECIDING, - DECIDED, -}; -using DecidingPathStatusWithStamp = std::pair; - -struct PreviousPullOverData -{ - struct SafetyStatus - { - std::optional safe_start_time{}; - bool is_safe{false}; - }; - - void reset() - { - safety_status = SafetyStatus{}; - deciding_path_status = DecidingPathStatusWithStamp{}; - } - - SafetyStatus safety_status{}; - DecidingPathStatusWithStamp deciding_path_status{}; -}; - -class ThreadSafeData -{ -public: - ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) - : mutex_(mutex), clock_(clock) - { - } - - bool incrementPathIndex() - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - if (pull_over_path_->incrementPathIndex()) { - last_path_idx_increment_time_ = clock_->now(); - return true; - } - return false; - } - - void set_pull_over_path(const PullOverPath & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - void set_pull_over_path(const std::shared_ptr & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - template - void set(Args... args) - { - std::lock_guard lock(mutex_); - (..., set_no_lock(args)); - } - - void clearPullOverPath() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - } - - bool foundPullOverPath() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return pull_over_path_->isValidPath(); - } - - PullOverPlannerType getPullOverPlannerType() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return PullOverPlannerType::NONE; - } - - return pull_over_path_->type; - }; - - void reset() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - goal_candidates_.clear(); - modified_goal_pose_ = std::nullopt; - last_path_update_time_ = std::nullopt; - last_path_idx_increment_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - last_previous_module_output_ = std::nullopt; - prev_data_.reset(); - } - - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) - - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) - DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data) - DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) - -private: - void set_pull_over_path_no_lock(const PullOverPath & path) - { - pull_over_path_ = std::make_shared(path); - if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } - - last_path_update_time_ = clock_->now(); - } - - void set_pull_over_path_no_lock(const std::shared_ptr & path) - { - pull_over_path_ = path; - if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } - last_path_update_time_ = clock_->now(); - } - - void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } - void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } - void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } - void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } - void set_no_lock(const PreviousPullOverData & arg) { prev_data_ = arg; } - void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; } - - std::shared_ptr pull_over_path_{nullptr}; - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - std::vector pull_over_path_candidates_; - GoalCandidates goal_candidates_{}; - std::optional modified_goal_pose_; - std::optional last_path_update_time_; - std::optional last_path_idx_increment_time_; - std::optional closest_start_pose_{}; - std::optional last_previous_module_output_{}; - PreviousPullOverData prev_data_{}; - CollisionCheckDebugMap collision_check_{}; - PredictedObjects static_target_objects_{}; - PredictedObjects dynamic_target_objects_{}; - - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; -}; - -#undef DEFINE_SETTER_WITH_MUTEX -#undef DEFINE_GETTER_WITH_MUTEX -#undef DEFINE_SETTER_GETTER_WITH_MUTEX - struct FreespacePlannerDebugData { bool is_planning{false}; @@ -312,12 +108,17 @@ struct PoseWithString struct PullOverContextData { PullOverContextData() = delete; - explicit PullOverContextData(const std::pair & is_safe_path) - : is_safe_path_latched(is_safe_path.first), is_safe_path_instant(is_safe_path.second) + explicit PullOverContextData( + const bool is_stable_safe_path, const PredictedObjects & static_objects, + const PredictedObjects & dynamic_objects) + : is_stable_safe_path(is_stable_safe_path), + static_target_objects(static_objects), + dynamic_target_objects(dynamic_objects) { } - bool is_safe_path_latched{true}; - bool is_safe_path_instant{true}; + const bool is_stable_safe_path; + const PredictedObjects static_target_objects; + const PredictedObjects dynamic_target_objects; }; class GoalPlannerModule : public SceneModuleInterface @@ -410,9 +211,6 @@ class GoalPlannerModule : public SceneModuleInterface initializeOccupancyGridMap(planner_data, parameters); }; GoalPlannerParameters parameters; - std::shared_ptr ego_predicted_path_params; - std::shared_ptr objects_filtering_params; - std::shared_ptr safety_check_params; autoware::universe_utils::LinearRing2d vehicle_footprint; PlannerData planner_data; @@ -428,12 +226,8 @@ class GoalPlannerModule : public SceneModuleInterface void updateOccupancyGrid(); GoalPlannerData clone() const; void update( - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params_, - const std::shared_ptr & objects_filtering_params_, - const std::shared_ptr & safety_check_params_, - const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const GoalPlannerParameters & parameters, const PlannerData & planner_data, + const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, const std::shared_ptr goal_searcher_, const autoware::universe_utils::LinearRing2d & vehicle_footprint); @@ -519,6 +313,8 @@ class GoalPlannerModule : public SceneModuleInterface // TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; + // path_decision_controller is updated in updateData(), and used in plan() + PathDecisionStateController path_decision_controller_{}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. @@ -548,11 +344,6 @@ class GoalPlannerModule : public SceneModuleInterface bool checkOccupancyGridCollision( const PathWithLaneId & path, const std::shared_ptr occupancy_grid_map) const; - bool checkObjectsCollision( - const PathWithLaneId & path, const std::vector & curvatures, - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, - const double collision_check_margin, const bool extract_static_objects, - const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -580,26 +371,11 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & current_pose, const double path_update_duration, const GoalPlannerParameters & parameters) const; bool isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const GoalPlannerParameters & parameters); - bool hasDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const; - bool hasNotDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const; - DecidingPathStatusWithStamp checkDecidingPathStatus( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const; void decideVelocity(); - bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); // validation @@ -617,13 +393,14 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr planner_data, const std::shared_ptr goal_searcher, const std::shared_ptr occupancy_grid_map); - bool canReturnToLaneParking(); + bool canReturnToLaneParking(const PullOverContextData & context_data); // plan pull over path BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data); std::optional> selectPullOverPath( + const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; @@ -633,7 +410,6 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); - void updatePreviousData(const PullOverContextData & context_data); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output); @@ -670,12 +446,12 @@ class GoalPlannerModule : public SceneModuleInterface */ /** * @brief Checks if the current path is safe. - * @return std::pair - * first: If the path is safe for a certain period of time, true. - * second: If the path is safe in the current state, true. + * @return If the path is safe in the current state, true. */ - std::pair isSafePath( - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + bool isSafePath( + const std::shared_ptr planner_data, const bool found_pull_over_path, + const std::optional & pull_over_path_opt, + const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index af2e756e64ca5..923fb6ae5bd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 8ba6239630497..6e2aaedd98b0e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -38,12 +38,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override - { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); - } + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index e3dccb3fc57ec..34743ae5fbf5f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -18,19 +18,14 @@ #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include -#include -#include #include #include -#include namespace autoware::behavior_path_planner { using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; -using autoware::freespace_planning_algorithms::AstarSearch; -using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOver : public PullOverPlannerBase { @@ -42,7 +37,7 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 2d7872707dd28..2dfcfb3dc6e9e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -43,7 +43,7 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCl() const { return planner_.getCl(); } std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; std::vector generatePullOverPaths( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index e9332041a8321..d952f8ddbd22d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -31,7 +31,6 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { enum class PullOverPlannerType { - NONE = 0, SHIFT, ARC_FORWARD, ARC_BACKWARD, @@ -40,137 +39,77 @@ enum class PullOverPlannerType { struct PullOverPath { - PullOverPlannerType type{PullOverPlannerType::NONE}; - std::vector partial_paths{}; - size_t path_idx{0}; - // accelerate with constant acceleration to the target velocity - std::vector> pairs_terminal_velocity_and_accel{}; - Pose start_pose{}; - Pose end_pose{}; - std::vector debug_poses{}; - size_t goal_id{}; - size_t id{}; - bool decided_velocity{false}; - - /** - * @brief Set paths and start/end poses - * By setting partial_paths, full_path, parking_path and curvature are also set at the same time - * @param input_partial_paths partial paths - * @param input_start_pose start pose - * @param input_end_pose end pose - */ - void setPaths( - const std::vector input_partial_paths, const Pose & input_start_pose, - const Pose & input_end_pose) - { - partial_paths = input_partial_paths; - start_pose = input_start_pose; - end_pose = input_end_pose; +public: + static std::optional create( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const Pose & end_pose, + const std::vector> & pairs_terminal_velocity_and_accel); - updatePathData(); - } + PullOverPath(const PullOverPath & other); - // Note: return copy value (not const&) because the value is used in multi threads - PathWithLaneId getFullPath() const { return full_path; } + PullOverPlannerType type() const { return type_; } + size_t goal_id() const { return goal_id_; } + size_t id() const { return id_; } + Pose start_pose() const { return start_pose_; } + Pose end_pose() const { return end_pose_; } - PathWithLaneId getParkingPath() const { return parking_path; } + std::vector & partial_paths() { return partial_paths_; } + const std::vector & partial_paths() const { return partial_paths_; } - PathWithLaneId getCurrentPath() const - { - if (partial_paths.empty()) { - return PathWithLaneId{}; - } else if (partial_paths.size() <= path_idx) { - return partial_paths.back(); - } - return partial_paths.at(path_idx); - } + // TODO(soblin): use reference to avoid copy once thread-safe design is finished + PathWithLaneId full_path() const { return full_path_; } + PathWithLaneId parking_path() const { return parking_path_; } + std::vector full_path_curvatures() { return full_path_curvatures_; } + std::vector parking_path_curvatures() const { return parking_path_curvatures_; } + double full_path_max_curvature() const { return full_path_max_curvature_; } + double parking_path_max_curvature() const { return parking_path_max_curvature_; } + size_t path_idx() const { return path_idx_; } - bool incrementPathIndex() - { - if (partial_paths.size() - 1 <= path_idx) { - return false; - } - path_idx += 1; - return true; - } + bool incrementPathIndex(); - bool isValidPath() const { return type != PullOverPlannerType::NONE; } + // TODO(soblin): this cannot be const due to decelerateBeforeSearchStart + PathWithLaneId & getCurrentPath(); - std::vector getFullPathCurvatures() const { return full_path_curvatures; } - std::vector getParkingPathCurvatures() const { return parking_path_curvatures; } - double getFullPathMaxCurvature() const { return full_path_max_curvature; } - double getParkingPathMaxCurvature() const { return parking_path_max_curvature; } + const PathWithLaneId & getCurrentPath() const; -private: - void updatePathData() + std::pair getPairsTerminalVelocityAndAccel() const { - updateFullPath(); - updateParkingPath(); - updateCurvatures(); - } - - void updateFullPath() - { - PathWithLaneId path{}; - for (size_t i = 0; i < partial_paths.size(); ++i) { - if (i == 0) { - path.points.insert( - path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); - } else { - // skip overlapping point - path.points.insert( - path.points.end(), next(partial_paths.at(i).points.begin()), - partial_paths.at(i).points.end()); - } + if (pairs_terminal_velocity_and_accel_.size() <= path_idx_) { + return std::make_pair(0.0, 0.0); } - full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + return pairs_terminal_velocity_and_accel_.at(path_idx_); } - void updateParkingPath() - { - if (full_path.points.empty()) { - updateFullPath(); - } - const size_t start_idx = - autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); - - PathWithLaneId path{}; - std::copy( - full_path.points.begin() + start_idx, full_path.points.end(), - std::back_inserter(path.points)); - parking_path = path; - } + std::vector debug_poses{}; - void updateCurvatures() - { - const auto calculateCurvaturesAndMax = - [](const auto & path) -> std::pair, double> { - std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); - double max_curvature = 0.0; - if (!curvatures.empty()) { - max_curvature = std::abs(*std::max_element( - curvatures.begin(), curvatures.end(), - [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); - } - return std::make_pair(curvatures, max_curvature); - }; - std::tie(full_path_curvatures, full_path_max_curvature) = - calculateCurvaturesAndMax(getFullPath()); - std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(getParkingPath()); - } +private: + PullOverPath( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const Pose & start_pose, const Pose & end_pose, + const std::vector & partial_paths, const PathWithLaneId & full_path, + const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel); + + PullOverPlannerType type_; + size_t goal_id_; + size_t id_; + Pose start_pose_; + Pose end_pose_; + + std::vector partial_paths_; + PathWithLaneId full_path_; + PathWithLaneId parking_path_; + std::vector full_path_curvatures_; + std::vector parking_path_curvatures_; + double full_path_max_curvature_; + double parking_path_max_curvature_; - // curvatures - std::vector full_path_curvatures{}; - std::vector parking_path_curvatures{}; - std::vector current_path_curvatures{}; - double parking_path_max_curvature{0.0}; - double full_path_max_curvature{0.0}; - double current_path_max_curvature{0.0}; - - // path - PathWithLaneId full_path{}; - PathWithLaneId parking_path{}; + // accelerate with constant acceleration to the target velocity + size_t path_idx_; + std::vector> pairs_terminal_velocity_and_accel_; }; class PullOverPlannerBase @@ -186,7 +125,7 @@ class PullOverPlannerBase virtual PullOverPlannerType getPlannerType() const = 0; virtual std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index c5640d9b4949f..9baceb4430dec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -36,7 +36,7 @@ class ShiftPullOver : public PullOverPlannerBase const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; protected: @@ -46,7 +46,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp new file mode 100644 index 0000000000000..3bfaa90b845fb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -0,0 +1,202 @@ +// Copyright 2024 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 AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ + +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } + +#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) + +class ThreadSafeData +{ +public: + ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) + : mutex_(mutex), clock_(clock) + { + } + + bool incrementPathIndex() + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + if (pull_over_path_->incrementPathIndex()) { + last_path_idx_increment_time_ = clock_->now(); + return true; + } + return false; + } + + void set_pull_over_path(const PullOverPath & path) + { + const std::lock_guard lock(mutex_); + set_pull_over_path_no_lock(path); + } + + void set_pull_over_path(const std::shared_ptr & path) + { + const std::lock_guard lock(mutex_); + set_pull_over_path_no_lock(path); + } + + template + void set(Args... args) + { + std::lock_guard lock(mutex_); + (..., set_no_lock(args)); + } + + void clearPullOverPath() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + } + + bool foundPullOverPath() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + return true; + } + + std::optional getPullOverPlannerType() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return std::nullopt; + } + return pull_over_path_->type(); + }; + + void reset() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + pull_over_path_candidates_.clear(); + goal_candidates_.clear(); + modified_goal_pose_ = std::nullopt; + last_path_update_time_ = std::nullopt; + last_path_idx_increment_time_ = std::nullopt; + closest_start_pose_ = std::nullopt; + last_previous_module_output_ = std::nullopt; + prev_data_ = PathDecisionState{}; + } + + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) + + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) + DEFINE_SETTER_GETTER_WITH_MUTEX( + utils::path_safety_checker::CollisionCheckDebugMap, collision_check) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) + +private: + void set_pull_over_path_no_lock(const PullOverPath & path) + { + pull_over_path_ = std::make_shared(path); + if (path.type() != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path_no_lock(const std::shared_ptr & path) + { + pull_over_path_ = path; + if (path->type() != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } + last_path_update_time_ = clock_->now(); + } + + void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } + void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } + void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } + void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } + void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg) + { + collision_check_ = arg; + } + + std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; + std::vector pull_over_path_candidates_; + GoalCandidates goal_candidates_{}; + std::optional modified_goal_pose_; + std::optional last_path_update_time_; + std::optional last_path_idx_increment_time_; + std::optional closest_start_pose_{}; + std::optional last_previous_module_output_{}; + utils::path_safety_checker::CollisionCheckDebugMap collision_check_{}; + PredictedObjects static_target_objects_{}; + PredictedObjects dynamic_target_objects_{}; + PathDecisionState prev_data_{}; + + std::recursive_mutex & mutex_; + rclcpp::Clock::SharedPtr clock_; +}; + +#undef DEFINE_SETTER_WITH_MUTEX +#undef DEFINE_GETTER_WITH_MUTEX +#undef DEFINE_SETTER_GETTER_WITH_MUTEX + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 70cdacf09f335..52dfbbddc79ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -17,7 +17,6 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include @@ -31,7 +30,6 @@ #include #include -#include #include #include @@ -129,6 +127,15 @@ bool isWithinAreas( */ std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes); +bool checkObjectsCollision( + const PathWithLaneId & path, const std::vector & curvatures, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const BehaviorPathPlannerParameters & behavior_path_parameters, + const double collision_check_margin, const bool extract_static_objects, + const double maximum_deceleration, + const double object_recognition_collision_check_max_extra_stopping_margin, + std::vector & ego_polygons_expanded, const bool update_debug_data = false); + // debug MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, @@ -152,6 +159,22 @@ std::string makePathPriorityDebugMessage( const std::map & path_id_to_rough_margin_map, const std::function & isSoftMargin, const std::function & isHighCurvature); +/** + * @brief combine two points + * @param points lane points + * @param points_next next lane points + * @return combined points + */ +lanelet::Points3d combineLanePoints( + const lanelet::Points3d & points, const lanelet::Points3d & points_next); +/** @brief Create a lanelet that represents the departure check area. + * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to. + * @param [in] route_handler RouteHandler object. + * @return Lanelet that goal footprints should be inside. + */ +lanelet::Lanelet createDepartureCheckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking); } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 2709dbe8635d3..160bb33dc07de 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -14,8 +14,6 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index dde3a554fca3d..147fe9f79dba0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -14,13 +14,17 @@ #include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" @@ -209,9 +213,6 @@ void GoalPlannerModule::onTimer() std::optional previous_module_output_opt{std::nullopt}; std::shared_ptr occupancy_grid_map{nullptr}; std::optional parameters_opt{std::nullopt}; - std::shared_ptr ego_predicted_path_params{nullptr}; - std::shared_ptr objects_filtering_params{nullptr}; - std::shared_ptr safety_check_params{nullptr}; std::shared_ptr goal_searcher{nullptr}; // begin of critical section @@ -224,23 +225,18 @@ void GoalPlannerModule::onTimer() previous_module_output_opt = gp_planner_data.previous_module_output; occupancy_grid_map = gp_planner_data.occupancy_grid_map; parameters_opt = gp_planner_data.parameters; - ego_predicted_path_params = gp_planner_data.ego_predicted_path_params; - objects_filtering_params = gp_planner_data.objects_filtering_params; - safety_check_params = gp_planner_data.safety_check_params; goal_searcher = gp_planner_data.goal_searcher; } } // end of critical section if ( !local_planner_data || !current_status_opt || !previous_module_output_opt || - !occupancy_grid_map || !parameters_opt || !ego_predicted_path_params || - !objects_filtering_params || !safety_check_params || !goal_searcher) { + !occupancy_grid_map || !parameters_opt || !goal_searcher) { RCLCPP_ERROR( getLogger(), "failed to get valid " "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt/" - "ego_predicted_path_params/objects_filtering_params/safety_check_params/goal_searcher in " - "onTimer"); + "goal_searcher in onTimer"); return; } const auto & current_status = current_status_opt.value(); @@ -261,6 +257,7 @@ void GoalPlannerModule::onTimer() } // check if new pull over path candidates are needed to be generated + const auto current_state = thread_safe_data_.get_prev_data().state; const bool need_update = std::invoke([&]() { if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { return false; @@ -276,15 +273,9 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } - // TODO(soblin): never call isSafePath on thread pool side - const auto local_context_data = PullOverContextData(isSafePath( - local_planner_data, parameters, ego_predicted_path_params, objects_filtering_params, - safety_check_params)); if ( hasDeviatedFromLastPreviousModulePath(local_planner_data) && - // TODO(soblin): use DecidingPathStatus in ThreadInputData - !hasDecidedPath( - local_planner_data, occupancy_grid_map, local_context_data, parameters, goal_searcher)) { + current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } @@ -311,22 +302,20 @@ void GoalPlannerModule::onTimer() const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - auto pull_over_path = - planner->plan(local_planner_data, previous_module_output, goal_candidate.goal_pose); - if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { - pull_over_path->goal_id = goal_candidate.id; - pull_over_path->id = path_candidates.size(); - + const auto pull_over_path = planner->plan( + goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output, + goal_candidate.goal_pose); + if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = - lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose).length; + lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose()).length; if (start_arc_length < min_start_arc_length) { min_start_arc_length = start_arc_length; // closest start pose is stop point when not finding safe path - closest_start_pose = pull_over_path->start_pose; + closest_start_pose = pull_over_path->start_pose(); } } }; @@ -428,7 +417,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; if ( - isStuck(local_planner_data, occupancy_grid_map, parameters) && is_new_costmap && + isStuck( + thread_safe_data_.get_static_target_objects(), thread_safe_data_.get_dynamic_target_objects(), + local_planner_data, occupancy_grid_map, parameters) && + is_new_costmap && needPathUpdate( local_planner_data->self_odometry->pose.pose, path_update_duration, parameters)) { planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); @@ -464,35 +456,33 @@ void GoalPlannerModule::updateData() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // extract static and dynamic objects in extraction polygon for path collision check - { - const auto & p = parameters_; - const auto & rh = *(planner_data_->route_handler); - const auto objects = *(planner_data_->dynamic_object); - const double vehicle_width = planner_data_->parameters.vehicle_width; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); - const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( - pull_over_lanes, left_side_parking_, p->detection_bound_offset, - p->margin_from_boundary + p->max_lateral_offset + vehicle_width); - if (objects_extraction_polygon.has_value()) { - debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); - } - PredictedObjects dynamic_target_objects{}; - for (const auto & object : objects.objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); - if ( - objects_extraction_polygon.has_value() && - boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { - dynamic_target_objects.objects.push_back(object); - } + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + const double vehicle_width = planner_data_->parameters.vehicle_width; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); + const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( + pull_over_lanes, left_side_parking_, p->detection_bound_offset, + p->margin_from_boundary + p->max_lateral_offset + vehicle_width); + if (objects_extraction_polygon.has_value()) { + debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); + } + PredictedObjects dynamic_target_objects{}; + for (const auto & object : objects.objects) { + const auto object_polygon = universe_utils::toPolygon2d(object); + if ( + objects_extraction_polygon.has_value() && + boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { + dynamic_target_objects.objects.push_back(object); } - const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, p->th_moving_object_velocity); - - // these objects are used for path collision check not for safety check - thread_safe_data_.set_static_target_objects(static_target_objects); - thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); } + const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_target_objects, p->th_moving_object_velocity); + + // these objects are used for path collision check not for safety check + thread_safe_data_.set_static_target_objects(static_target_objects); + thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). @@ -518,8 +508,7 @@ void GoalPlannerModule::updateData() // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) // and if these two coincided, only the reference count is affected gp_planner_data.update( - *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_, - *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, + *parameters_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, vehicle_footprint_); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since @@ -550,9 +539,22 @@ void GoalPlannerModule::updateData() resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - context_data_ = PullOverContextData(isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_)); + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path = + found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + + const bool is_current_safe = isSafePath( + planner_data_, found_pull_over_path, pull_over_path, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_); + path_decision_controller_.transit_state( + found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, + thread_safe_data_.get_modified_goal_pose(), planner_data_, occupancy_grid_map_, is_current_safe, + *parameters_, goal_searcher_, isActivated(), pull_over_path, debug_data_.ego_polygons_expanded); + + context_data_.emplace( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects); // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { @@ -561,6 +563,8 @@ void GoalPlannerModule::updateData() thread_safe_data_.set_goal_candidates(generateGoalCandidates()); } + thread_safe_data_.set_prev_data(path_decision_controller_.get_current_state()); + if (!isActivated()) { return; } @@ -572,6 +576,7 @@ void GoalPlannerModule::updateData() if (!last_approval_data_) { last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); + // TODO(soblin): do not "plan" in updateData decideVelocity(); } } @@ -683,11 +688,11 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // NOTE(soblin): at least in goal_planner, isExecutionReady is called via super::updateRTCStatus + // from self::postProcess, so returning cached member variable like + // path_decision_controller.get_current_state() is valid if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { - if (!isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_) - .first) { + if (!path_decision_controller_.get_current_state().is_stable_safe) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; } @@ -794,10 +799,10 @@ bool GoalPlannerModule::planFreespacePath( if (!goal_candidate.is_safe) { continue; } - auto freespace_path = freespace_planner_->plan( - planner_data, BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK + const auto freespace_path = freespace_planner_->plan( + goal_candidate.id, 0, planner_data, + BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK goal_candidate.goal_pose); - freespace_path->goal_id = goal_candidate.id; if (!freespace_path) { continue; } @@ -817,7 +822,7 @@ bool GoalPlannerModule::planFreespacePath( return false; } -bool GoalPlannerModule::canReturnToLaneParking() +bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking if (!isStopped()) { @@ -829,14 +834,17 @@ bool GoalPlannerModule::canReturnToLaneParking() return false; } - const PathWithLaneId path = lane_parking_path->getFullPath(); - const std::vector curvatures = lane_parking_path->getFullPathCurvatures(); + const PathWithLaneId path = lane_parking_path->full_path(); + const std::vector curvatures = lane_parking_path->full_path_curvatures(); if ( parameters_->use_object_recognition && - checkObjectsCollision( - path, curvatures, planner_data_, *parameters_, + goal_planner_utils::checkObjectsCollision( + path, curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, + planner_data_->parameters, parameters_->object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false)) { + /*extract_static_objects=*/false, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded)) { return false; } @@ -888,7 +896,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() } const auto context_data = context_data_.has_value() ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(context_data); @@ -899,6 +907,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() } std::optional> GoalPlannerModule::selectPullOverPath( + const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const { @@ -924,7 +933,7 @@ std::optional> GoalPlannerModule::selectP } for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { const auto & path = pull_over_path_candidates[i]; - const auto goal_candidate_it = goal_candidate_map.find(path.goal_id); + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { sorted_path_indices.push_back(i); } @@ -969,14 +978,14 @@ std::optional> GoalPlannerModule::selectP [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; - return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + return goal_id_to_index[a.goal_id()] < goal_id_to_index[b.goal_id()]; }); // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { const auto & order = parameters_->efficient_path_order; - const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); - const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; @@ -998,13 +1007,13 @@ std::optional> GoalPlannerModule::selectP for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( - path.getParkingPath(), target_objects, planner_data_->parameters, false, "max"); + path.parking_path(), target_objects, planner_data_->parameters, false, "max"); auto it = std::lower_bound( margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { - path_id_to_rough_margin_map[path.id] = margins_with_zero.back(); + path_id_to_rough_margin_map[path.id()] = margins_with_zero.back(); } else { - path_id_to_rough_margin_map[path.id] = *it; + path_id_to_rough_margin_map[path.id()] = *it; } } @@ -1015,27 +1024,29 @@ std::optional> GoalPlannerModule::selectP const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; if ( - std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01) { + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01) { return false; } - return path_id_to_rough_margin_map[a.id] > path_id_to_rough_margin_map[b.id]; + return path_id_to_rough_margin_map[a.id()] > path_id_to_rough_margin_map[b.id()]; }); // STEP2-3: Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_rough_margin_map[path.id]; + const double margin = path_id_to_rough_margin_map[path.id()]; return std::any_of( soft_margins.begin(), soft_margins.end(), [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); }; const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01; + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01; }; // NOTE: this is just partition sort based on curvature threshold within each sub partitions @@ -1103,12 +1114,16 @@ std::optional> GoalPlannerModule::selectP parameters_->object_recognition_collision_check_hard_margins.back(); for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; - const PathWithLaneId parking_path = path.getParkingPath(); - const auto parking_path_curvatures = path.getParkingPathCurvatures(); + const PathWithLaneId & parking_path = path.parking_path(); + const auto & parking_path_curvatures = path.parking_path_curvatures(); if ( - parameters_->use_object_recognition && checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data_, - *parameters_, collision_check_margin, true, true)) { + parameters_->use_object_recognition && + goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, context_data.static_target_objects, + context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin, + true, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded, true)) { continue; } if ( @@ -1116,7 +1131,7 @@ std::optional> GoalPlannerModule::selectP checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id))); + return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id()))); } return {}; } @@ -1152,7 +1167,7 @@ void GoalPlannerModule::setOutput( } if ( - parameters_->safety_check_params.enable_safety_check && !context_data.is_safe_path_latched && + parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk @@ -1177,8 +1192,8 @@ void GoalPlannerModule::setOutput( // set hazard and turn signal if ( - hasDecidedPath( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) && + path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::DECIDED && isActivated()) { setTurnSignalInfo(output); } @@ -1188,7 +1203,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt && planner_type_opt.value() == 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; @@ -1245,145 +1261,14 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } -bool GoalPlannerModule::hasDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const -{ - return checkDecidingPathStatus( - planner_data, occupancy_grid_map, context_data, parameters, goal_searcher) - .first == DecidingPathStatus::DECIDED; -} - -bool GoalPlannerModule::hasNotDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const -{ - return checkDecidingPathStatus( - planner_data, occupancy_grid_map, context_data, parameters, goal_searcher) - .first == DecidingPathStatus::NOT_DECIDED; -} - -DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const -{ - const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; - const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; - - // Once this function returns true, it will continue to return true thereafter - if (prev_status.first == DecidingPathStatus::DECIDED) { - return prev_status; - } - - // if path is not safe, not decided - if (!thread_safe_data_.foundPullOverPath()) { - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // If it is dangerous against dynamic objects before approval, do not determine the path. - // This eliminates a unsafe path to be approved - if (enable_safety_check && !context_data.is_safe_path_latched && !isActivated()) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " - "approval"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if object recognition for path collision check is enabled, transition to DECIDED after - // DECIDING for a certain period of time if there is no collision. - const auto pull_over_path = thread_safe_data_.get_pull_over_path(); - const auto current_path = pull_over_path->getCurrentPath(); - if (prev_status.first == DecidingPathStatus::DECIDING) { - const double hysteresis_factor = 0.9; - - // check goal pose collision - const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); - if ( - modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, - planner_data, thread_safe_data_.get_static_target_objects())) { - RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // check current parking path collision - const auto parking_path = pull_over_path->getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path->getParkingPathCurvatures(); - const double margin = - parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; - if (checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data, parameters, margin, - /*extract_static_objects=*/false)) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - if (enable_safety_check && !context_data.is_safe_path_latched) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if enough time has passed since deciding status starts, transition to DECIDED - constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); - if (elapsed_time_from_deciding > check_collision_duration) { - RCLCPP_DEBUG( - getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); - return {DecidingPathStatus::DECIDED, clock_->now()}; - } - - // if enough time has NOT passed since deciding status starts, keep DECIDING - RCLCPP_DEBUG( - getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", - elapsed_time_from_deciding); - return prev_status; - } - - // 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 size_t ego_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - - const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - current_path.points, pull_over_path->start_pose.position); - const double dist_to_parking_start_pose = calcSignedArcLength( - current_path.points, current_pose.position, ego_segment_idx, - pull_over_path->start_pose.position, start_pose_segment_idx); - if (dist_to_parking_start_pose > parameters.decide_path_distance) { - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if object recognition for path collision check is enabled, transition to DECIDING to check - // collision for a certain period of time. Otherwise, transition to DECIDED directly. - if (parameters.use_object_recognition) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " - "period of time"); - return {DecidingPathStatus::DECIDING, clock_->now()}; - } - RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED"); - return {DecidingPathStatus::DECIDED, clock_->now()}; -} - void GoalPlannerModule::decideVelocity() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); + // partial_paths + auto & first_path = thread_safe_data_.get_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) { @@ -1395,9 +1280,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // TODO(soblin): move DecidingPathStatus to context_data - if (!hasDecidedPath( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_)) { + if ( + path_decision_controller_.get_current_state().state != + PathDecisionState::DecisionKind::DECIDED) { return planPullOverAsCandidate(context_data); } @@ -1450,9 +1335,13 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( return getPreviousModuleOutput(); } + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + const bool is_freespace = + planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE; if ( - hasNotDecidedPath( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) && + path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::NOT_DECIDED && + !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, @@ -1470,7 +1359,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // Select a path that is as safe as possible and has a high priority. const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); - const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates); + const auto path_and_goal_opt = + selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ if (path_and_goal_opt) { @@ -1478,7 +1368,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( deceleratePath(pull_over_path); thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); RCLCPP_DEBUG( - getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id, + getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), modified_goal.id); } else { thread_safe_data_.set(goal_candidates); @@ -1490,9 +1380,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( setOutput(context_data, output); // return to lane parking if it is possible - if ( - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && - canReturnToLaneParking()) { + if (is_freespace && canReturnToLaneParking(context_data)) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } @@ -1509,9 +1397,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( } path_candidate_ = - std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - - updatePreviousData(context_data); + std::make_shared(thread_safe_data_.get_pull_over_path()->full_path()); return output; } @@ -1525,12 +1411,10 @@ void GoalPlannerModule::postProcess() } const auto context_data = context_data_.has_value() ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); - // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a - // waste of time because it gives same result throughout the main thread. const bool has_decided_path = - hasDecidedPath(planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_); + path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; context_data_ = std::nullopt; @@ -1545,46 +1429,12 @@ void GoalPlannerModule::postProcess() } updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose, + {thread_safe_data_.get_pull_over_path()->start_pose(), thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); -} - -void GoalPlannerModule::updatePreviousData(const PullOverContextData & context_data) -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - // for the next loop setOutput(). - // this is used to determine whether to generate a new stop path or keep the current stop path. - // TODO(Mamoru Sobue): put prev_data_ out of ThreadSafeData - auto prev_data = thread_safe_data_.get_prev_data(); - - prev_data.deciding_path_status = checkDecidingPathStatus( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_); - - // This is related to safety check, so if it is disabled, return here. - if (!parameters_->safety_check_params.enable_safety_check) { - prev_data.safety_status.is_safe = true; - } else { - // Even if the current path is safe, it will not be safe unless it stands for a certain period - // of time. Record the time when the current path has become safe - const auto is_safe = context_data.is_safe_path_latched, - current_is_safe = context_data.is_safe_path_instant; - if (current_is_safe) { - if (!prev_data.safety_status.safe_start_time) { - prev_data.safety_status.safe_start_time = clock_->now(); - } - } else { - prev_data.safety_status.safe_start_time = std::nullopt; - } - prev_data.safety_status.is_safe = is_safe; - } - - // commit the change - thread_safe_data_.set_prev_data(prev_data); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1597,7 +1447,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() } const auto context_data = context_data_.has_value() ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(context_data); @@ -1615,7 +1465,7 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto full_path = thread_safe_data_.get_pull_over_path()->full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1625,10 +1475,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); + full_path.points, thread_safe_data_.get_pull_over_path()->start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( @@ -1697,7 +1547,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const std::invoke([&]() -> std::optional> { if (thread_safe_data_.foundPullOverPath()) { return std::make_pair( - thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + thread_safe_data_.get_pull_over_path()->start_pose(), "stop at selected start pose"); } if (thread_safe_data_.get_closest_start_pose()) { return std::make_pair( @@ -1811,6 +1661,7 @@ bool GoalPlannerModule::isStopped() } bool GoalPlannerModule::isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const GoalPlannerParameters & parameters) @@ -1839,10 +1690,12 @@ bool GoalPlannerModule::isStuck( if (parameters.use_object_recognition) { const auto path = pull_over_path->getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); - if (checkObjectsCollision( - path, curvatures, planner_data, parameters, + if (goal_planner_utils::checkObjectsCollision( + path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters, parameters.object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false)) { + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded)) { return true; } } @@ -1913,12 +1766,12 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto path = thread_safe_data_.get_pull_over_path()->full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; - const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; + const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose(); + const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -1962,12 +1815,13 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_lane_change = false; constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::SHIFT) { return false; } constexpr double distance_threshold = 1.0; const auto stop_point = - thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); + thread_safe_data_.get_pull_over_path()->partial_paths().front().points.back(); const double distance_from_ego_to_stop_point = std::abs(autoware::motion_utils::calcSignedArcLength( path.points, stop_point.point.pose.position, current_pose.position)); @@ -1993,77 +1847,6 @@ bool GoalPlannerModule::checkOccupancyGridCollision( return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); } -bool GoalPlannerModule::checkObjectsCollision( - const PathWithLaneId & path, const std::vector & curvatures, - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, - const double collision_check_margin, const bool extract_static_objects, - const bool update_debug_data) const -{ - const auto target_objects = extract_static_objects - ? thread_safe_data_.get_static_target_objects() - : thread_safe_data_.get_dynamic_target_objects(); - if (target_objects.objects.empty()) { - return false; - } - - // check collision roughly with {min_distance, max_distance} between ego footprint and objects - // footprint - std::pair has_collision_rough = - utils::path_safety_checker::checkObjectsCollisionRough( - path, target_objects, collision_check_margin, planner_data_->parameters, false); - if (!has_collision_rough.first) { - return false; - } - if (has_collision_rough.second) { - return true; - } - - std::vector obj_polygons; - for (const auto & object : target_objects.objects) { - obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); - } - - /* Expand ego collision check polygon - * - `collision_check_margin` is added in all directions. - * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. - * - `extra_lateral_margin` adds the lateral margin on curves. - */ - if (path.points.size() != curvatures.size()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); - return false; - } - std::vector ego_polygons_expanded{}; - for (size_t i = 0; i < path.points.size(); ++i) { - const auto p = path.points.at(i); - const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters.maximum_deceleration, - parameters.object_recognition_collision_check_max_extra_stopping_margin); - - // The square is meant to imply centrifugal force, but it is not a very well-founded formula. - // TODO(kosuke55): It is needed to consider better way because there is an inherently - // different conception of the inside and outside margins. - const double extra_lateral_margin = std::min( - extra_stopping_margin, - std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - - const auto ego_polygon = autoware::universe_utils::toFootprint( - p.point.pose, - planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, - planner_data->parameters.base_link2rear + collision_check_margin, - planner_data->parameters.vehicle_width + collision_check_margin * 2.0 + - extra_lateral_margin * 2.0); - ego_polygons_expanded.push_back(ego_polygon); - } - - if (update_debug_data) { - debug_data_.ego_polygons_expanded = ego_polygons_expanded; - } - - return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); -} - bool GoalPlannerModule::hasEnoughDistance( const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const { @@ -2075,9 +1858,9 @@ bool GoalPlannerModule::hasEnoughDistance( // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = pull_over_path.partial_paths.size() > 1; + const bool is_separated_path = pull_over_path.partial_paths().size() > 1; const double distance_to_start = calcSignedArcLength( - long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose.position); + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); const double distance_to_restart = parameters_->decide_path_distance / 2; const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; @@ -2141,9 +1924,9 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( thread_safe_data_.get_goal_candidates(), planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, + pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); - auto & first_path = pull_over_path.partial_paths.front(); + auto & first_path = pull_over_path.partial_paths().front(); if (decel_pose) { decelerateBeforeSearchStart(*decel_pose, first_path); return; @@ -2307,8 +2090,8 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); - const Pose & start_pose = pull_over_path.start_pose; - const Pose & end_pose = pull_over_path.end_pose; + 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); } @@ -2368,16 +2151,18 @@ static std::vector filterOb return refined_filtered_objects; } -std::pair GoalPlannerModule::isSafePath( - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, +bool GoalPlannerModule::isSafePath( + const std::shared_ptr planner_data, const bool found_pull_over_path, + const std::optional & pull_over_path_opt, const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const { - if (!thread_safe_data_.get_pull_over_path()) { - return {false, false}; + if (!found_pull_over_path || !pull_over_path_opt) { + return false; } - const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); + const auto & pull_over_path = pull_over_path_opt.value(); + const auto & current_pull_over_path = pull_over_path.getCurrentPath(); const auto & current_pose = planner_data->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data->self_odometry->twist.twist.linear.x, @@ -2390,11 +2175,9 @@ std::pair GoalPlannerModule::isSafePath( const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters.backward_goal_search_length, parameters.forward_goal_search_length); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(pull_over_path.points); + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(current_pull_over_path.points); const std::pair terminal_velocity_and_accel = - utils::parking_departure::getPairsTerminalVelocityAndAccel( - thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, - thread_safe_data_.get_pull_over_path()->path_idx); + pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); @@ -2405,8 +2188,8 @@ std::pair GoalPlannerModule::isSafePath( const bool limit_to_max_velocity = true; const auto ego_predicted_path = autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, pull_over_path.points, current_pose, current_velocity, ego_seg_idx, - is_object_front, limit_to_max_velocity); + ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, + ego_seg_idx, is_object_front, limit_to_max_velocity); // ========================================================================================== // if ego is before the entry of pull_over_lanes, the beginning of the safety check area @@ -2436,8 +2219,8 @@ std::pair GoalPlannerModule::isSafePath( first_road_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( - calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < - 0.0) { + calcSignedArcLength( + current_pull_over_path.points, first_road_pose.position, current_pose.position) < 0.0) { return first_road_pose; } // if current ego pose is in pull over lanes segment, use current ego pose @@ -2459,13 +2242,13 @@ std::pair GoalPlannerModule::isSafePath( const auto prev_data = thread_safe_data_.get_prev_data(); const double hysteresis_factor = - prev_data.safety_status.is_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; + prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; CollisionCheckDebugMap collision_check{}; const bool current_is_safe = std::invoke([&]() { if (parameters.safety_check_params.method == "RSS") { return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_over_path, ego_predicted_path, filtered_objects, collision_check, + current_pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, objects_filtering_params->use_all_predicted_path, hysteresis_factor, safety_check_params->collision_check_yaw_diff_threshold); @@ -2496,16 +2279,7 @@ std::pair GoalPlannerModule::isSafePath( * | | | | | | | | * 0 =========================-------==========-- t */ - if (current_is_safe) { - if ( - prev_data.safety_status.safe_start_time && - (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds() > - parameters.safety_check_params.keep_unsafe_time) { - return {true /*is_safe*/, true /*current_is_safe*/}; - } - } - - return {false /*is_safe*/, current_is_safe}; + return current_is_safe; } void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) @@ -2536,10 +2310,10 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = - hasDecidedPath(planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) - ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const auto color = path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::DECIDED + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); @@ -2580,18 +2354,18 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, + thread_safe_data_.get_pull_over_path()->start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + thread_safe_data_.get_pull_over_path()->full_path(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray( thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths.size(); ++i) { - const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths.at(i); + for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths().size(); ++i) { + const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths().at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -2664,17 +2438,17 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { const auto prev_data = thread_safe_data_.get_prev_data(); visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "safety_status", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = planner_data_->self_odometry->pose.pose; - marker.text += "is_safe: " + std::to_string(prev_data.safety_status.is_safe) + "\n"; - if (prev_data.safety_status.safe_start_time) { + marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; + if (prev_data.safe_start_time) { const double elapsed_time_from_safe_start = - (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds(); + (clock_->now() - prev_data.safe_start_time.value()).seconds(); marker.text += "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; } @@ -2695,14 +2469,17 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) marker.pose = thread_safe_data_.get_modified_goal_pose() ? thread_safe_data_.get_modified_goal_pose()->goal_pose : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); - if (thread_safe_data_.foundPullOverPath()) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt) { + marker.text = magic_enum::enum_name(planner_type_opt.value()); marker.text += - " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx) + "/" + - std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); + " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx()) + "/" + + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths().size() - 1); } - if (isStuck(planner_data_, occupancy_grid_map_, *parameters_)) { + if (isStuck( + context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, + occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; } else if (isStopped()) { marker.text += " stopped"; @@ -2789,25 +2566,18 @@ GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() c { GoalPlannerModule::GoalPlannerData gp_planner_data(planner_data, parameters); gp_planner_data.update( - parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, - planner_data, current_status, previous_module_output, goal_searcher, vehicle_footprint); + parameters, planner_data, current_status, previous_module_output, goal_searcher, + vehicle_footprint); return gp_planner_data; } void GoalPlannerModule::GoalPlannerData::update( - const GoalPlannerParameters & parameters_, - const std::shared_ptr & ego_predicted_path_params_, - const std::shared_ptr & objects_filtering_params_, - const std::shared_ptr & safety_check_params_, - const PlannerData & planner_data_, const ModuleStatus & current_status_, - const BehaviorModuleOutput & previous_module_output_, + const GoalPlannerParameters & parameters_, const PlannerData & planner_data_, + const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, const std::shared_ptr goal_searcher_, const autoware::universe_utils::LinearRing2d & vehicle_footprint_) { parameters = parameters_; - ego_predicted_path_params = ego_predicted_path_params_; - objects_filtering_params = objects_filtering_params_; - safety_check_params = safety_check_params_; vehicle_footprint = vehicle_footprint_; planner_data = planner_data_; @@ -2822,4 +2592,148 @@ void GoalPlannerModule::GoalPlannerData::update( goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal()); } +void PathDecisionStateController::transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded) +{ + const auto next_state = get_next_state( + found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, + planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, + pull_over_path, ego_polygons_expanded); + prev_state_ = current_state_; + current_state_ = next_state; +} + +PathDecisionState PathDecisionStateController::get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const +{ + auto next_state = prev_state_; + + // update timestamp + next_state.stamp = now; + + // update safety + if (!parameters.safety_check_params.enable_safety_check) { + next_state.is_stable_safe = true; + } else { + if (is_current_safe) { + if (!next_state.safe_start_time) { + next_state.safe_start_time = now; + next_state.is_stable_safe = false; + } else { + next_state.is_stable_safe = + ((now - next_state.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time); + } + } else { + next_state.safe_start_time = std::nullopt; + next_state.is_stable_safe = false; + } + } + + // Once this function returns true, it will continue to return true thereafter + if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { + next_state.state = PathDecisionState::DecisionKind::DECIDED; + return next_state; + } + + // if path is not safe, not decided + if (!found_pull_over_path || !pull_over_path_opt) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & pull_over_path = pull_over_path_opt.value(); + const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; + // If it is dangerous against dynamic objects before approval, do not determine the path. + // This eliminates a unsafe path to be approved + if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & current_path = pull_over_path.getCurrentPath(); + if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + if ( + modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, + planner_data, static_target_objects)) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // check current parking path collision + const auto & parking_path = pull_over_path.parking_path(); + const std::vector parking_path_curvatures = pull_over_path.parking_path_curvatures(); + const double margin = + parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; + if (goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, static_target_objects, dynamic_target_objects, + planner_data->parameters, margin, + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + ego_polygons_expanded, true)) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + if (enable_safety_check && !next_state.is_stable_safe) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if enough time has passed since deciding status starts, transition to DECIDED + constexpr double check_collision_duration = 1.0; + const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); + if (elapsed_time_from_deciding > check_collision_duration) { + next_state.state = PathDecisionState::DecisionKind::DECIDED; + return next_state; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + return next_state; + } + + // 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 size_t ego_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); + + const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + current_path.points, pull_over_path.start_pose().position); + const double dist_to_parking_start_pose = calcSignedArcLength( + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path.start_pose().position, start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters.decide_path_distance) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters.use_object_recognition) { + next_state.state = PathDecisionState::DecisionKind::DECIDED; + return next_state; + } + return {PathDecisionState::DecisionKind::DECIDED, now}; +} + } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index b9351e4588d5a..ea0816954d0e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -15,13 +15,11 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" #include @@ -33,7 +31,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; @@ -119,11 +116,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); - auto lanes = utils::getExtendedCurrentLanes( - planner_data, backward_length, forward_length, - /*forward_only_in_route*/ false); - lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); - + const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *route_handler, left_side_parking_); const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); @@ -194,7 +188,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p break; } - if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { + if (!boost::geometry::within( + transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) { continue; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 32d6c55276876..34248a75eb1f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -14,7 +14,8 @@ #include "autoware/behavior_path_goal_planner_module/manager.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include @@ -26,6 +27,13 @@ namespace autoware::behavior_path_planner { +std::unique_ptr GoalPlannerModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); +} + GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( rclcpp::Node * node, const std::string & base_ns) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 4b377aed46f7c..dbdac08c8778c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -20,11 +20,18 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include + #include #include namespace autoware::behavior_path_planner { + +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; + FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) @@ -50,7 +57,7 @@ FreespacePullOver::FreespacePullOver( } std::optional FreespacePullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const Pose & current_pose = planner_data->self_odometry->pose.pose; @@ -138,11 +145,12 @@ std::optional FreespacePullOver::plan( } } - PullOverPath pull_over_path{}; - pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; - pull_over_path.setPaths(partial_paths, current_pose, goal_pose); - pull_over_path.type = getPlannerType(); - - return pull_over_path; + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose, + pairs_terminal_velocity_and_accel); + if (!pull_over_path_opt) { + return {}; + } + return pull_over_path_opt.value(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index eecdc11937463..6c4aee5b96abf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -38,7 +38,7 @@ GeometricPullOver::GeometricPullOver( } std::optional GeometricPullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const auto & route_handler = planner_data->route_handler; @@ -72,11 +72,12 @@ std::optional GeometricPullOver::plan( // check lane departure with road and shoulder lanes if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; - PullOverPath pull_over_path{}; - pull_over_path.type = getPlannerType(); - pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); - pull_over_path.setPaths(planner_.getPaths(), planner_.getStartPose(), planner_.getArcEndPose()); - - return pull_over_path; + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(), + planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel()); + if (!pull_over_path_opt) { + return {}; + } + return pull_over_path_opt.value(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp new file mode 100644 index 0000000000000..f6535e7adb8f8 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -0,0 +1,154 @@ +// Copyright 2024 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 autoware::behavior_path_planner +{ + +std::optional PullOverPath::create( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, const Pose & end_pose, + const std::vector> & pairs_terminal_velocity_and_accel) +{ + if (partial_paths.empty()) { + return std::nullopt; + } + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + PathWithLaneId full_path{}; + full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + if (full_path.points.size() < 3) { + return std::nullopt; + } + + const size_t start_idx = + autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId parking_path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(parking_path.points)); + + if (parking_path.points.size() < 3) { + return std::nullopt; + } + + const auto calculateCurvaturesAndMax = + [](const auto & path) -> std::pair, double> { + std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); + double max_curvature = 0.0; + if (!curvatures.empty()) { + max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + } + return std::make_pair(curvatures, max_curvature); + }; + + std::vector full_path_curvatures{}; + std::vector parking_path_curvatures{}; + double full_path_max_curvature{}; + double parking_path_max_curvature{}; + std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); + std::tie(parking_path_curvatures, parking_path_max_curvature) = + calculateCurvaturesAndMax(full_path); + + return PullOverPath( + type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, + full_path_curvatures, parking_path_curvatures, full_path_max_curvature, + parking_path_max_curvature, pairs_terminal_velocity_and_accel); +} + +PullOverPath::PullOverPath(const PullOverPath & other) +: type_(other.type_), + goal_id_(other.goal_id_), + id_(other.id_), + start_pose_(other.start_pose_), + end_pose_(other.end_pose_), + partial_paths_(other.partial_paths_), + full_path_(other.full_path_), + parking_path_(other.parking_path_), + full_path_curvatures_(other.full_path_curvatures_), + parking_path_curvatures_(other.parking_path_curvatures_), + full_path_max_curvature_(other.full_path_max_curvature_), + parking_path_max_curvature_(other.parking_path_max_curvature_), + path_idx_(other.path_idx_), + pairs_terminal_velocity_and_accel_(other.pairs_terminal_velocity_and_accel_) +{ +} + +PullOverPath::PullOverPath( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose, + const Pose & end_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel) +: type_(type), + goal_id_(goal_id), + id_(id), + start_pose_(start_pose), + end_pose_(end_pose), + partial_paths_(partial_paths), + full_path_(full_path), + parking_path_(parking_path), + full_path_curvatures_(full_path_curvatures), + parking_path_curvatures_(parking_path_curvatures), + full_path_max_curvature_(full_path_max_curvature), + parking_path_max_curvature_(parking_path_max_curvature), + path_idx_(0), + pairs_terminal_velocity_and_accel_(pairs_terminal_velocity_and_accel) +{ +} + +bool PullOverPath::incrementPathIndex() +{ + { + if (partial_paths_.size() - 1 <= path_idx_) { + return false; + } + path_idx_ += 1; + return true; + } +} + +PathWithLaneId & PullOverPath::getCurrentPath() +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +const PathWithLaneId & PullOverPath::getCurrentPath() const +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 4536fc8873b40..645d74b6385da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -35,7 +35,7 @@ ShiftPullOver::ShiftPullOver( { } std::optional ShiftPullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const auto & route_handler = planner_data->route_handler; @@ -59,7 +59,8 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, lateral_jerk); + goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, + lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -126,7 +127,7 @@ std::optional ShiftPullOver::cropPrevModulePath( } std::optional ShiftPullOver::generatePullOverPath( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -254,13 +255,14 @@ std::optional ShiftPullOver::generatePullOverPath( } // set pull over path - PullOverPath pull_over_path{}; - pull_over_path.type = getPlannerType(); - std::vector partial_paths{shifted_path.path}; - pull_over_path.setPaths( - partial_paths, path_shifter.getShiftLines().front().start, - path_shifter.getShiftLines().front().end); - pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)}); + + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); pull_over_path.debug_poses.push_back(goal_pose); @@ -275,7 +277,7 @@ std::optional ShiftPullOver::generatePullOverPath( const auto parking_lot_polygons = lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr()); const auto path_footprints = goal_planner_utils::createPathFootPrints( - pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width); + pull_over_path.parking_path(), p.base_link2front, p.base_link2rear, p.vehicle_width); const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { return std::any_of( parking_lot_polygons.begin(), parking_lot_polygons.end(), @@ -299,7 +301,7 @@ std::optional ShiftPullOver::generatePullOverPath( const auto combined_drivable = utils::combineDrivableLanes( expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); return !lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); + utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); }); if (!is_in_parking_lots && !is_in_lanes) { return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 01247e11aef3e..3815acc4561e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" @@ -33,7 +33,6 @@ #include #include -#include #include #include @@ -299,6 +298,80 @@ std::vector getBusStopAreaPolygons(const lanelet::Const return area_polygons; } +bool checkObjectsCollision( + const PathWithLaneId & path, const std::vector & curvatures, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const BehaviorPathPlannerParameters & behavior_path_parameters, + const double collision_check_margin, const bool extract_static_objects, + const double maximum_deceleration, + const double object_recognition_collision_check_max_extra_stopping_margin, + std::vector & debug_ego_polygons_expanded, const bool update_debug_data) +{ + if (path.points.size() != curvatures.size()) { + RCLCPP_WARN( + rclcpp::get_logger("goal_planner_util"), + "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); + return false; + } + + const auto & target_objects = + extract_static_objects ? static_target_objects : dynamic_target_objects; + if (target_objects.objects.empty()) { + return false; + } + + // check collision roughly with {min_distance, max_distance} between ego footprint and objects + // footprint + std::pair has_collision_rough = + utils::path_safety_checker::checkObjectsCollisionRough( + path, target_objects, collision_check_margin, behavior_path_parameters, false); + if (!has_collision_rough.first) { + return false; + } + if (has_collision_rough.second) { + return true; + } + + std::vector obj_polygons; + for (const auto & object : target_objects.objects) { + obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); + } + + /* Expand ego collision check polygon + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. + */ + std::vector ego_polygons_expanded{}; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, + object_recognition_collision_check_max_extra_stopping_margin); + + // The square is meant to imply centrifugal force, but it is not a very well-founded formula. + // TODO(kosuke55): It is needed to consider better way because there is an inherently + // different conception of the inside and outside margins. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); + + const auto ego_polygon = autoware::universe_utils::toFootprint( + p.point.pose, + behavior_path_parameters.base_link2front + collision_check_margin + extra_stopping_margin, + behavior_path_parameters.base_link2rear + collision_check_margin, + behavior_path_parameters.vehicle_width + collision_check_margin * 2.0 + + extra_lateral_margin * 2.0); + ego_polygons_expanded.push_back(ego_polygon); + } + + if (update_debug_data) { + debug_ego_polygons_expanded = ego_polygons_expanded; + } + + return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); +} + MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) @@ -596,20 +669,75 @@ std::string makePathPriorityDebugMessage( for (size_t i = 0; i < sorted_path_indices.size(); ++i) { const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; // goal_index is same to goal priority including unsafe goal - const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id())); const bool is_safe_goal = goal_candidates[goal_index].is_safe; - const int goal_priority = goal_id_and_priority[path.goal_id]; + const int goal_priority = goal_id_and_priority[path.goal_id()]; - ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id << ", goal_id: " << path.goal_id + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type()) + << ", path_id: " << path.id() << ", goal_id: " << path.goal_id() << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_rough_margin_map.at(path.id) + << ", margin: " << path_id_to_rough_margin_map.at(path.id()) << (isSoftMargin(path) ? " (soft)" : " (hard)") - << ", curvature: " << path.getParkingPathMaxCurvature() + << ", curvature: " << path.parking_path_max_curvature() << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; } ss << "-----------------------------------------------------------\n"; return ss.str(); } +lanelet::Points3d combineLanePoints( + const lanelet::Points3d & points, const lanelet::Points3d & points_next) +{ + lanelet::Points3d combined_points; + std::unordered_set point_ids; + for (const auto & point : points) { + if (point_ids.insert(point.id()).second) { + combined_points.push_back(point); + } + } + for (const auto & point : points_next) { + if (point_ids.insert(point.id()).second) { + combined_points.push_back(point); + } + } + return combined_points; +} + +lanelet::Lanelet createDepartureCheckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking) +{ + const auto getBoundPoints = + [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d { + lanelet::Points3d points; + const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound()) + : (is_outer ? lane.rightBound() : lane.leftBound()); + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet { + return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true) + : route_handler.getMostLeftLanelet(lane, false, true); + }; + + lanelet::Points3d outer_bound_points{}; + lanelet::Points3d inner_bound_points{}; + for (const auto & lane : pull_over_lanes) { + const auto current_outer_bound_points = getBoundPoints(lane, true); + const auto most_inner_lane = getMostInnerLane(lane); + const auto current_inner_bound_points = getBoundPoints(most_inner_lane, false); + outer_bound_points = combineLanePoints(outer_bound_points, current_outer_bound_points); + inner_bound_points = combineLanePoints(inner_bound_points, current_inner_bound_points); + } + + const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points); + const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points); + return lanelet::Lanelet( + lanelet::InvalId, left_side_parking ? outer_linestring : inner_linestring, + left_side_parking ? inner_linestring : outer_linestring); +} + } // namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp index ca93505fe1eae..b50ee083fddc8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -82,3 +82,86 @@ TEST_F(TestUtilWithMap, isWithinAreas) baselink_footprint, bus_stop_area_polygons), true); } + +TEST_F(TestUtilWithMap, combineLanePoints) +{ + // 1) combine points with no duplicate IDs + { + lanelet::Points3d points{ + {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}}; + lanelet::Points3d points_next{ + {lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0), lanelet::Point3d(6, 0, 0)}}; + + const auto combined_points = + autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next); + EXPECT_EQ(combined_points.size(), 6); + } + + // 2) combine points with duplicate IDs + { + lanelet::Points3d points{ + {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}}; + lanelet::Points3d points_next{ + {lanelet::Point3d(3, 0, 0), lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0)}}; + + const auto combined_points = + autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next); + EXPECT_EQ(combined_points.size(), 5); + } +} + +TEST_F(TestUtilWithMap, createDepartureCheckLanelet) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + + const geometry_msgs::msg::Pose goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build() + .x(433.42254638671875) + .y(465.3381652832031) + .z(0.0)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(0.306785474523741) + .w(0.9517786888879384)); + + // 1) get target shoulder lane and check it's lane id + const auto target_shoulder_lane = route_handler->getPullOverTarget(goal_pose); + EXPECT_EQ(target_shoulder_lane.has_value(), true); + EXPECT_EQ(target_shoulder_lane.value().id(), 18391); + + // 2) get shoulder lane sequence + const auto target_shoulder_lanes = + route_handler->getShoulderLaneletSequence(target_shoulder_lane.value(), goal_pose); + EXPECT_EQ(target_shoulder_lanes.size(), 3); + + // 3) check if the right bound of the departure check lane extended to the right end matches the + // right bound of the rightmost lanelet + const auto to_points3d = [](const lanelet::ConstLineString3d & bound) { + lanelet::Points3d points; + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto departure_check_lane = + autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet( + target_shoulder_lanes, *route_handler, true); + const auto departure_check_lane_right_bound_points = + to_points3d(departure_check_lane.rightBound()); + + const std::vector most_right_lanelet_ids = {18381, 18383, 18388}; + lanelet::Points3d right_bound_points; + for (const auto & id : most_right_lanelet_ids) { + const auto lanelet = lanelet_map_ptr->laneletLayer.get(id); + right_bound_points = autoware::behavior_path_planner::goal_planner_utils::combineLanePoints( + right_bound_points, to_points3d(lanelet.rightBound())); + } + + EXPECT_EQ(departure_check_lane_right_bound_points.size(), right_bound_points.size()); + for (size_t i = 0; i < departure_check_lane_right_bound_points.size(); ++i) { + EXPECT_EQ(departure_check_lane_right_bound_points.at(i).id(), right_bound_points.at(i).id()); + } +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5f1c3debabcf8..18a797976161c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -24,6 +24,108 @@ The lane change candidate path is divided into two phases: preparation and lane- ![lane-change-phases](./images/lane_change-lane_change_phases.png) +The following chart illustrates the process of sampling candidate paths for lane change. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (Is current lanes, target lanes OR target neighbor lanes polygon empty?) then (yes) + #LightPink:Return prev approved path; + stop +else (no) +endif + +:Get target objects; + +:Calculate prepare phase metrics; + +:Start loop through prepare phase metrics; + +repeat + :Get prepare segment; + + if (Is LC start point outside target lanes range) then (yes) + if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop + else (no) + #LightPink:Return prev approved path; + stop + endif + else (no) + endif + + :Calculate shift phase metrics; + + :Start loop through shift phase metrics; + repeat + :Get candidate path; + note left: Details shown in the next chart + if (Is valid candidate path?) then (yes) + :Store candidate path; + if (Is candidate path safe?) then (yes) + #LightGreen:Return candidate path; + stop + else (no) + endif + else (no) + endif + repeat while (Finished looping shift phase metrics) is (FALSE) + repeat while (Is finished looping prepare phase metrics) is (FALSE) + +if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop +else (no) +endif + +#LightPink:Return prev approved path; +stop + +@enduml +``` + +While the following chart demonstrates the process of generating a valid candidate path. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +:Calculate resample interval; + +:Get reference path from target lanes; + +if (Is reference path empty?) then (yes) + #LightPink:Return; + stop +else (no) +endif + +:Get LC shift line; + +:Set lane change Info; +note left: set information from sampled prepare phase and shift phase metrics\n(duration, length, velocity, and acceleration) + +:Construct candidate path; + +if (Candidate path has enough length?) then (yes) + #LightGreen:Return valid candidate path; + stop +else (no) + #LightPink:Return; + stop +endif + +@enduml +``` + ### Preparation phase 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. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 4c55beb89e813..0a2c9991ec5fd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -34,6 +34,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + enable_target_lane_bound_check: true collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 3f4f290663a3f..f76d776150875 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -54,6 +54,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_transient_data() final; + void update_filtered_objects() final; void updateLaneChangeStatus() override; @@ -153,18 +155,17 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; - bool hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; - - bool hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - bool hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + LaneChangePath get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double target_lane_length, const double shift_length, + const double next_lc_buffer, const bool is_goal_in_route) const; - bool getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const; + bool check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, + const double lane_change_buffer, const bool is_stuck) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index b237368692312..72a40caca1d6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -67,6 +67,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_transient_data() = 0; + virtual void update_filtered_objects() = 0; virtual void updateLaneChangeStatus() = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 32b104eb23abb..021fa16fa6ec1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -24,23 +24,8 @@ using autoware::route_handler::Direction; using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; - -/** - * @brief Calculates the distance from the ego vehicle to the terminal point. - * - * This function computes the distance from the current position of the ego vehicle - * to either the goal pose or the end of the current lane, depending on whether - * the vehicle's current lane is within the goal section. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non null `lanes_ptr` that points to the current lanes data. - * - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle. - * - Non null `route_handler_ptr` that contains the goal pose of the route. - * - * @return The distance to the terminal point (either the goal pose or the end of the current lane) - * in meters. - */ -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); +using behavior_path_planner::lane_change::MinMaxValue; +using behavior_path_planner::lane_change::PhaseMetrics; double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, @@ -118,13 +103,6 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); - -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); - double calc_maximum_lane_change_length( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); @@ -132,6 +110,68 @@ double calc_maximum_lane_change_length( double calc_maximum_lane_change_length( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc); + +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration); + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, + const std::vector & lon_accel_values, const double current_velocity, + const double min_length_threshold = 0.0, + const double max_length_threshold = std::numeric_limits::max()); + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_velocity, const double lon_accel, + const double max_length_threshold = std::numeric_limits::max()); + +/** + * @brief Calculates the minimum and maximum lane changing lengths, along with the corresponding + * distance buffers. + * + * This function computes the minimum and maximum lane change lengths based on shift intervals and + * vehicle parameters. It only accounts for the lane changing phase itself, excluding the prepare + * distance, which should be handled separately during path generation. + * + * Additionally, the function calculates the distance buffer to ensure that the ego vehicle has + * enough space to complete the lane change, including cases where multiple lane changes may be + * necessary. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes: + * - `lc_param_ptr`: Parameters related to lane changing, such as velocity, lateral acceleration, + * and jerk. + * - `route_handler_ptr`: Pointer to the route handler for managing routes. + * - `direction`: Lane change direction (e.g., left or right). + * - `transient_data.acc.max`: Maximum acceleration of the vehicle. + * - Other relevant data for the ego vehicle's dynamics and state. + * @param lanes The set of lanelets representing the lanes for the lane change. + * + * @return A pair of `MinMaxValue` structures where: + * - The first element contains the minimum and maximum lane changing lengths in meters. + * - The second element contains the minimum and maximum distance buffers in meters. + * If the lanes or necessary data are unavailable, returns `std::numeric_limits::max()` for + * both values. + */ +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 194412dfe01b7..fc5e78e44b77f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -18,15 +18,16 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include +#include #include #include -#include #include #include #include +#include #include #include #include @@ -74,8 +75,8 @@ struct LateralAccelerationMap 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); + const double min_acc = autoware::interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = autoware::interpolation::lerp(base_vel, base_max_acc, velocity); return std::make_pair(min_acc, max_acc); } @@ -154,6 +155,7 @@ struct Parameters // safety check bool allow_loose_check_for_cancel{true}; + bool enable_target_lane_bound_check{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_parked{}; @@ -192,9 +194,32 @@ struct PhaseInfo } }; +struct PhaseMetrics +{ + double duration{0.0}; + double length{0.0}; + double velocity{0.0}; + double sampled_lon_accel{0.0}; + double actual_lon_accel{0.0}; + double lat_accel{0.0}; + + PhaseMetrics( + const double _duration, const double _length, const double _velocity, + const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel) + : duration(_duration), + length(_length), + velocity(_velocity), + sampled_lon_accel(_sampled_lon_accel), + actual_lon_accel(_actual_lon_accel), + lat_accel(_lat_accel) + { + } +}; + struct Lanes { bool current_lane_in_goal_section{false}; + bool target_lane_in_goal_section{false}; lanelet::ConstLanelets current; lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; @@ -215,6 +240,23 @@ struct Info double lateral_acceleration{0.0}; double terminal_lane_changing_velocity{0.0}; + + Info() = default; + Info( + const PhaseMetrics & _prep_metrics, const PhaseMetrics & _lc_metrics, + const Pose & _lc_start_pose, const Pose & _lc_end_pose, const ShiftLine & _shift_line) + { + longitudinal_acceleration = + PhaseInfo{_prep_metrics.actual_lon_accel, _lc_metrics.actual_lon_accel}; + duration = PhaseInfo{_prep_metrics.duration, _lc_metrics.duration}; + velocity = PhaseInfo{_prep_metrics.velocity, _prep_metrics.velocity}; + length = PhaseInfo{_prep_metrics.length, _lc_metrics.length}; + lane_changing_start = _lc_start_pose; + lane_changing_end = _lc_end_pose; + lateral_acceleration = _lc_metrics.lat_accel; + terminal_lane_changing_velocity = _lc_metrics.velocity; + shift_line = _shift_line; + } }; template @@ -273,6 +315,32 @@ struct LanesPolygon std::vector preceding_target; }; +struct MinMaxValue +{ + double min{std::numeric_limits::infinity()}; + double max{std::numeric_limits::infinity()}; +}; + +struct TransientData +{ + MinMaxValue acc; // acceleration profile for accelerating lane change path + MinMaxValue lane_changing_length; // lane changing length for a single lane change + MinMaxValue + current_dist_buffer; // distance buffer computed backward from current lanes' terminal end + MinMaxValue + next_dist_buffer; // distance buffer computed backward from target lanes' terminal end + double dist_to_terminal_end{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal end + double dist_to_terminal_start{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal start + double max_prepare_length{ + std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + + bool is_ego_near_current_terminal_start{false}; +}; + using RouteHandlerPtr = std::shared_ptr; using BppParamPtr = std::shared_ptr; using LCParamPtr = std::shared_ptr; @@ -287,12 +355,14 @@ struct CommonData LCParamPtr lc_param_ptr; LanesPtr lanes_ptr; LanesPolygonPtr lanes_polygon_ptr; + TransientData transient_data; + PathWithLaneId current_lanes_path; ModuleType lc_type; Direction direction; - [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + [[nodiscard]] const Pose & get_ego_pose() const { return self_odometry_ptr->pose.pose; } - [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + [[nodiscard]] const Twist & get_ego_twist() const { return self_odometry_ptr->twist.twist; } [[nodiscard]] double get_ego_speed(bool use_norm = false) const { @@ -304,6 +374,18 @@ struct CommonData const auto y = get_ego_twist().linear.y; return std::hypot(x, y); } + + [[nodiscard]] bool is_data_available() const + { + return route_handler_ptr && self_odometry_ptr && bpp_param_ptr && lc_param_ptr && lanes_ptr && + lanes_polygon_ptr; + } + + [[nodiscard]] bool is_lanes_available() const + { + return lanes_ptr && !lanes_ptr->current.empty() && !lanes_ptr->target.empty() && + !lanes_ptr->target_neighbor.empty(); + } }; using CommonDataPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change @@ -316,6 +398,7 @@ using LaneChangeModuleType = lane_change::ModuleType; using LaneChangeParameters = lane_change::Parameters; using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; using FilteredByLanesObjects = lane_change::LanesObjects>; using FilteredByLanesExtendedObjects = lane_change::LanesObjects; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 84c3f310f0382..363fa970f54c4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -45,6 +45,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVeloc using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; @@ -68,10 +69,6 @@ double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); @@ -94,17 +91,16 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -std::optional constructCandidatePath( +bool path_footprint_exceeds_target_lane_bound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin = 0.1); + +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length); - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); @@ -125,11 +121,6 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); -bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & curent_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction); - CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); std::optional getLaneChangeTargetLane( @@ -217,8 +208,9 @@ rclcpp::Logger getLogger(const std::string & type); * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); +Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info); + +Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left); /** * @brief Checks if the given polygon is within an intersection area. @@ -254,33 +246,12 @@ bool isWithinIntersection( */ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); -/** - * @brief Calculates the distance required during a lane change operation. - * - * Used for computing prepare or lane change length based on current and maximum velocity, - * acceleration, and duration, returning the lesser of accelerated distance or distance at max - * velocity. - * - * @param velocity The current velocity of the vehicle in meters per second (m/s). - * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). - * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). - * @param duration The duration of the lane change in seconds (s). - * @return The calculated minimum distance in meters (m). - */ -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double time); - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); bool is_same_lane_with_prev_iteration( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation); - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object); @@ -300,13 +271,4 @@ double get_distance_to_next_regulatory_element( const bool ignore_intersection = false); } // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); - -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset); -} // namespace autoware::behavior_path_planner::utils::lane_change::debug - #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 7ddafdbd6c935..0b647acd44268 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); + module_type_->update_transient_data(); module_type_->update_filtered_objects(); module_type_->updateSpecialData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 69df2fe14317a..dd7382d8f7116 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); p.collision_check_yaw_diff_threshold = getOrDeclareParameter( *node, parameter("safety_check.collision_check_yaw_diff_threshold")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index e59119230c224..a91aaba86bb1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -86,11 +86,17 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target = target_lanes; const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); + common_data_ptr_->lanes_ptr->target_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(target_lanes.back()); + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); @@ -98,6 +104,53 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_transient_data() +{ + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { + return; + } + + auto & transient_data = common_data_ptr_->transient_data; + std::tie(transient_data.acc.min, transient_data.acc.max) = calcCurrentMinMaxAcceleration(); + + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); + + transient_data.next_dist_buffer.min = + transient_data.current_dist_buffer.min - transient_data.lane_changing_length.min - + common_data_ptr_->lc_param_ptr->lane_change_finish_judge_buffer; + + transient_data.dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + transient_data.dist_to_terminal_start = + transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + + transient_data.is_ego_near_current_terminal_start = + transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + + RCLCPP_DEBUG( + logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); + RCLCPP_DEBUG( + logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min, + transient_data.lane_changing_length.max); + RCLCPP_DEBUG( + logger_, "current_dist_buffer - min: %.4f, max: %.4f", transient_data.current_dist_buffer.min, + transient_data.current_dist_buffer.max); + RCLCPP_DEBUG( + logger_, "next_dist_buffer - min: %.4f, max: %.4f", transient_data.next_dist_buffer.min, + transient_data.next_dist_buffer.max); + RCLCPP_DEBUG(logger_, "dist_to_terminal_start: %.4f", transient_data.dist_to_terminal_start); + RCLCPP_DEBUG(logger_, "dist_to_terminal_end: %.4f", transient_data.dist_to_terminal_end); + RCLCPP_DEBUG(logger_, "max_prepare_length: %.4f", transient_data.max_prepare_length); + RCLCPP_DEBUG( + logger_, "is_ego_near_current_terminal_start: %s", + (transient_data.is_ego_near_current_terminal_start ? "true" : "false")); +} + void NormalLaneChange::update_filtered_objects() { filtered_objects_ = filterObjects(); @@ -128,9 +181,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) } LaneChangePaths valid_paths{}; - const bool is_stuck = isVehicleStuck(current_lanes); - bool found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, is_stuck, &valid_paths); + bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin lane_change_debug_.valid_paths = valid_paths; @@ -153,24 +204,20 @@ bool NormalLaneChange::isLaneChangeRequired() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto current_lanes = - utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - - if (current_lanes.empty()) { + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto ego_dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - if (ego_dist_to_target_start > maximum_prepare_length) { + if (ego_dist_to_target_start > max_prepare_length) { return false; } @@ -184,21 +231,10 @@ bool NormalLaneChange::isLaneChangeRequired() bool NormalLaneChange::is_near_regulatory_element() const { - const auto & current_lanes = get_current_lanes(); + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - if (current_lanes.empty()) return false; - - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); - - if (shift_intervals.empty()) return false; - - const auto & lc_params = *common_data_ptr_->lc_param_ptr; const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto min_lc_length = - calculation::calc_minimum_lane_change_length(lc_params, shift_intervals); - const auto dist_to_terminal_start = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_) - min_lc_length; + const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; if (dist_to_terminal_start <= max_prepare_length) return false; @@ -251,14 +287,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); - const double next_lane_change_buffer = - calculation::calc_minimum_lane_change_length(lane_change_param, shift_intervals); - - const double buffer = next_lane_change_buffer + - lane_change_param.min_length_for_turn_signal_activation + - common_param.base_link2front; + const auto buffer = common_data_ptr_->transient_data.current_dist_buffer.min + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); @@ -407,9 +438,8 @@ void NormalLaneChange::insertStopPoint( return; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const auto lane_change_buffer = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + const auto [_, lanes_dist_buffer] = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, lanelets); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -427,7 +457,7 @@ void NormalLaneChange::insertStopPoint( const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const auto target_objects = filterObjects(); - double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + double stopping_distance = distance_to_terminal - lanes_dist_buffer.min - stop_point_buffer; const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { @@ -453,14 +483,14 @@ void NormalLaneChange::insertStopPoint( for (const auto & object : target_objects.current_lane) { // check if stationary - const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + const auto obj_v = std::abs(object.initial_twist.linear.x); if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { continue; } // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); for (const auto & polygon_p : polygon) { const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); @@ -488,11 +518,9 @@ void NormalLaneChange::insertStopPoint( const auto rss_dist = calcRssDistance( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); - const double lane_change_buffer_for_blocking_object = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + distance_to_ego_lane_obj - lanes_dist_buffer.min - lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - getCommonParam().base_link2front; @@ -507,21 +535,21 @@ void NormalLaneChange::insertStopPoint( const bool has_blocking_target_lane_obj = std::any_of( target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), [&](const auto & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); + const auto v = std::abs(o.initial_twist.linear.x); if (v > lane_change_parameters_->stopped_object_velocity_threshold) { return false; } // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj; }); @@ -706,9 +734,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + // TODO(Azu) fully change to transient data const auto distance_to_lane_change_end = std::invoke([&]() { auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -718,7 +745,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); } - return std::max(0.0, distance_to_end) - lane_change_buffer; + return std::max(0.0, distance_to_end) - + common_data_ptr_->transient_data.current_dist_buffer.min; }); lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; @@ -813,27 +841,18 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const bool NormalLaneChange::is_near_terminal() const { - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - - if (current_lanes.empty()) { + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) { return true; } - const auto & current_lanes_terminal = current_lanes.back(); + // TODO(Azu) fully change to transient data const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto direction = common_data_ptr_->direction; - const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - const auto min_lane_changing_distance = calculation::calc_minimum_lane_change_length( - route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); - const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); - const auto min_lc_dist_with_buffer = - backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; - const auto dist_from_ego_to_terminal_end = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto min_lc_dist_with_buffer = backward_buffer + current_min_dist_buffer; - return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; + return common_data_ptr_->transient_data.dist_to_terminal_end < min_lc_dist_with_buffer; } bool NormalLaneChange::isEgoOnPreparePhase() const @@ -956,10 +975,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // calculate maximum lane change length - const double max_lane_change_length = + // TODO(Azu) Double check why it's failing with transient data + const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -981,12 +1001,12 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); - if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { RCLCPP_DEBUG( logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } - } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { RCLCPP_DEBUG( logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; @@ -1315,7 +1335,7 @@ PathWithLaneId NormalLaneChange::getTargetSegment( bool NormalLaneChange::hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction) const + const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const { if (target_lanes.empty()) { return false; @@ -1325,8 +1345,7 @@ bool NormalLaneChange::hasEnoughLength( const auto & route_handler = getRouteHandler(); const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); const auto minimum_lane_change_length_to_preferred_lane = - calculation::calc_minimum_lane_change_length( - route_handler, target_lanes.back(), *lane_change_parameters_, direction); + common_data_ptr_->transient_data.next_dist_buffer.min; const double lane_change_length = path.info.length.sum(); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -1351,76 +1370,31 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - - const double dist_to_crosswalk_from_lane_change_start_pose = - utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - - path.info.length.prepare; - // Check lane changing section includes crosswalk - if ( - dist_to_crosswalk_from_lane_change_start_pose > 0.0 && - dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { - return false; - } + lane_change_debug_.collision_check_objects.clear(); - return true; -} + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + const auto is_stuck = isVehicleStuck(current_lanes); -bool NormalLaneChange::hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; - const double dist_to_intersection_from_lane_change_start_pose = - utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; - // Check lane changing section includes intersection + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if ( - dist_to_intersection_from_lane_change_start_pose > 0.0 && - dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return false; } - return true; -} - -bool NormalLaneChange::hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto dist_to_next_traffic_light = - getDistanceToNextTrafficLight(current_pose, current_lanes); - const auto dist_to_next_traffic_light_from_lc_start_pose = - dist_to_next_traffic_light - path.info.length.prepare; - - return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || - dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; -} - -bool NormalLaneChange::getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const -{ - lane_change_debug_.collision_check_objects.clear(); - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto backward_path_length = common_parameters.backward_path_length; - const auto forward_path_length = common_parameters.forward_path_length; - const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; - const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; - // get velocity const auto current_velocity = getEgoVelocity(); @@ -1428,36 +1402,28 @@ bool NormalLaneChange::getLaneChangePaths( const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto dist_to_end_of_current_lanes = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; + const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto & target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - candidate_paths->reserve( - longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + candidate_paths.reserve( + longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * + prepare_durations.size()); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", @@ -1468,261 +1434,240 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - for (const auto & prepare_duration : prepare_durations) { - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::clamp( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity, getCommonParam().max_vel); + const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, dist_to_terminal_start); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 - : ((prepare_velocity - current_velocity) / prepare_duration); + auto check_length_diff = + [&](const double prep_length, const double lc_length, const bool check_lc) { + if (candidate_paths.empty()) return true; - const auto prepare_length = utils::lane_change::calcPhaseLength( - current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); + if (prep_diff > lane_change_parameters_->skip_process_lon_diff_th_prepare) return true; - const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer; - if (prepare_length > ego_dist_to_terminal_start) { - RCLCPP_DEBUG( - logger_, - "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f, ego dist to " - "terminal start: %.5f", - prepare_length, ego_dist_to_terminal_start); - continue; - } + if (!check_lc) return false; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) { - RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); - continue; - } + const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); + return lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing; + }; + + for (const auto & prep_metric : prepare_phase_metrics) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG( + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, + prep_metric.actual_lon_accel, prep_metric.length); + }; + + if (!check_length_diff(prep_metric.length, 0.0, false)) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); + continue; + } + + auto prepare_segment = + getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); + + if (prepare_segment.points.empty()) { + debug_print("prepare segment is empty...? Unexpected."); + continue; + } + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { + debug_print( + "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + continue; + } + + // lane changing start 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( + current_lanes, target_lanes.front(), lane_changing_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + debug_print("lane change start is behind target lanelet!"); + break; + } + + debug_print("Prepare path satisfy constraints"); + + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + dist_to_terminal_start > max_prepare_length + ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length + : dist_to_terminal_end - prep_metric.length; + auto target_lane_buffer = + lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; + if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; } - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); - const auto debug_print = [&](const auto & s) { + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); + + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); + + for (const auto & lc_metric : lane_changing_metrics) { + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s, - prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length); + logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), + lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); }; - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); + if (!check_length_diff(prep_metric.length, lc_metric.length, true)) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); continue; } - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + LaneChangePath candidate_path; + try { + candidate_path = get_candidate_path( + prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, + target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); 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( - current_lanes, target_lanes.front(), lane_changing_start_pose); + candidate_paths.push_back(candidate_path); - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start getEgoPose() is behind target lanelet!"); - break; + bool is_safe = false; + try { + is_safe = check_candidate_path_safety( + candidate_path, target_objects, current_min_dist_buffer, is_stuck); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); + return false; } - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->lane_change_lat_acc_map.find(initial_lane_changing_velocity); - const auto lateral_acc_resolution = - std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - - std::vector sample_lat_acc; - constexpr double eps = 0.01; - for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { - sample_lat_acc.push_back(a); + if (is_safe) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; } - RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); - - debug_print("Prepare path satisfy constraints"); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - for (const auto & lateral_acc : sample_lat_acc) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = utils::lane_change::calcPhaseLength( - initial_lane_changing_velocity, getCommonParam().max_vel, - longitudinal_acc_on_lane_changing, lane_changing_time); - - const auto debug_print_lat = [&](const auto & s) { - RCLCPP_DEBUG( - logger_, - " - %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s, - lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing, - lane_changing_length); - }; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - const auto lc_length_diff = - candidate_paths->back().info.length.lane_changing - lane_changing_length; - - // We only check lc_length_diff if and only if the current prepare_length is equal to the - // previous prepare_length. - if ( - std::abs(prev_prep_diff) < eps && - std::abs(lc_length_diff) < - lane_change_parameters_->skip_process_lon_diff_th_lane_changing) { - RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); - continue; - } - } - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } + debug_print_lat("Reject: sampled path is not safe."); + } + } - if ( - ego_dist_to_terminal_start > max_prepare_length && - lane_changing_length + prepare_length > dist_to_next_regulatory_element) { - debug_print_lat( - "Reject: length of lane changing path is longer than length to regulatory element!!"); - continue; - } + RCLCPP_DEBUG(logger_, "No safety path found."); + return false; +} - // if multiple lane change is necessary, does the remaining distance is sufficient - const auto remaining_dist_in_target = std::invoke([&]() { - const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; - const auto num_to_preferred_lane_from_target_lane = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const auto backward_len_buffer = - lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto backward_buffer_to_target_lane = - num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer; - return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane + - next_lane_change_buffer; - }); - - if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } +LaneChangePath NormalLaneChange::get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double target_lane_length, const double shift_length, + const double next_lc_buffer, const bool is_goal_in_route) const +{ + const auto & route_handler = *getRouteHandler(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & forward_path_length = planner_data_->parameters.forward_path_length; - const auto terminal_lane_changing_velocity = std::min( - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, - getCommonParam().max_vel); - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); + const auto resample_interval = + utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, + forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("target_lane_reference_path is empty!"); + } - if (target_segment.points.empty()) { - debug_print_lat("Reject: target segment is empty!! something wrong..."); - continue; - } + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); - - if (target_lane_reference_path.points.empty()) { - debug_print_lat("Reject: target_lane_reference_path is empty!!"); - continue; - } + const auto shift_line = utils::lane_change::get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); + LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - const auto candidate_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, prepare_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + const auto candidate_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - if (!candidate_path) { - debug_print_lat("Reject: failed to generate candidate path!!"); - continue; - } + if (!candidate_path) { + throw std::logic_error("failed to generate candidate path!"); + } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print_lat("Reject: invalid candidate path!!"); - continue; - } + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + throw std::logic_error("invalid candidate path length!"); + } - candidate_paths->push_back(*candidate_path); + return *candidate_path; +} - if ( - !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { - debug_print_lat( - "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " - "lane change."); - return false; - } +bool NormalLaneChange::check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, + const double lane_change_buffer, const bool is_stuck) const +{ + if ( + !is_stuck && !utils::lane_change::passed_parked_objects( + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, + lane_change_buffer, lane_change_debug_.collision_check_objects)) { + throw std::logic_error( + "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); + } - const auto is_safe = std::invoke([&]() { - constexpr size_t decel_sampling_num = 1; - const auto safety_check_with_normal_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); - - if (!safety_check_with_normal_rss.is_safe && is_stuck) { - const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); - return safety_check_with_stuck_rss.is_safe; - } - - return safety_check_with_normal_rss.is_safe; - }); - - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } + const auto lc_start_velocity = candidate_path.info.velocity.prepare; + const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity; + constexpr double margin = 0.1; + // path is unsafe if it exceeds target lane boundary with a high velocity + if ( + lane_change_parameters_->enable_target_lane_bound_check && + lc_start_velocity > min_lc_velocity + margin && + utils::lane_change::path_footprint_exceeds_target_lane_bound( + common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { + throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); + } - debug_print_lat("Reject: sampled path is not safe."); - } - } + constexpr size_t decel_sampling_num = 1; + const auto safety_check_with_normal_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, + lane_change_debug_.collision_check_objects); + + if (!safety_check_with_normal_rss.is_safe && is_stuck) { + const auto safety_check_with_stuck_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, + decel_sampling_num, lane_change_debug_.collision_check_objects); + return safety_check_with_stuck_rss.is_safe; } - RCLCPP_DEBUG(logger_, "No safety path found."); - return false; + return safety_check_with_normal_rss.is_safe; } std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; + + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; + if ( + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return {}; } + const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; @@ -1732,25 +1677,14 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return {}; - } - // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); @@ -1763,7 +1697,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( prev_module_output_.path.points, current_lane_terminal_point, - -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); @@ -1789,35 +1723,26 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, - minimum_lane_changing_velocity, next_lane_change_buffer); + target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, + minimum_lane_changing_velocity, next_min_dist_buffer); if (target_segment.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); return {}; } - const lanelet::BasicPoint2d lc_start_point( - lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - - const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); - - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - LaneChangeInfo lane_change_info; lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - if (!is_valid_start_point) { + if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " @@ -1826,18 +1751,18 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_change_buffer, minimum_lane_changing_velocity); + current_min_dist_buffer, minimum_lane_changing_velocity); const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_min_dist_buffer); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); return {}; } - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( lane_changing_start_pose.value(), target_segment.points.front().point.pose, target_lane_reference_path, shift_length); @@ -1850,9 +1775,9 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points.pop_back(); reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; - const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, reference_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, + sorted_lane_ids); return terminal_lane_change_path; } @@ -1871,12 +1796,11 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto min_lc_length = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, + debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -1992,16 +1916,14 @@ bool NormalLaneChange::calcAbortPath() 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 minimum_lane_change_length = calculation::calc_minimum_lane_change_length( - route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto & lane_changing_path = selected_path.path; const auto & reference_lanelets = get_current_lanes(); 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); + lanelet::utils::getLaneletLength2d(reference_lanelets) - current_min_dist_buffer, 0.0); const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( @@ -2041,9 +1963,12 @@ bool NormalLaneChange::calcAbortPath() return false; } - if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - route_handler, reference_lanelets, current_pose, abort_return_dist, - *lane_change_parameters_, direction)) { + const auto enough_abort_dist = + abort_start_dist + abort_return_dist + + calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <= + common_data_ptr_->transient_data.dist_to_terminal_start; + + if (!enough_abort_dist) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -2196,10 +2121,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns( utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= - lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = + (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_param; return is_collided( lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); }); @@ -2293,10 +2218,10 @@ bool NormalLaneChange::isVehicleStuck( const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary continue; } @@ -2313,10 +2238,9 @@ bool NormalLaneChange::isVehicleStuck( route_handler->isInGoalRouteSection(current_lanes.back()) ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; + const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { return true; } @@ -2350,8 +2274,7 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan } const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto max_lane_change_length = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); + const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; const auto rss_dist = calcRssDistance( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); @@ -2362,9 +2285,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = max_lane_change_length + rss_dist + + const auto detection_distance = current_max_dist_buffer + rss_dist + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + RCLCPP_DEBUG( + logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index c5424f69f84e0..1c4aede51074a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -20,13 +20,12 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation { -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) -{ - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & current_lanes = lanes_ptr->current; - const auto & current_pose = common_data_ptr->get_ego_pose(); - return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose); +rclcpp::Logger get_logger() +{ + constexpr const char * name{"lane_change.utils"}; + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; } double calc_dist_from_pose_to_terminal_end( @@ -37,10 +36,10 @@ double calc_dist_from_pose_to_terminal_end( return 0.0; } - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - - if (lanes_ptr->current_lane_in_goal_section) { + const auto in_goal_route_section = + common_data_ptr->route_handler_ptr->isInGoalRouteSection(lanes.back()); + if (in_goal_route_section) { + const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); return utils::getSignedDistance(src_pose, goal_pose, lanes); } return utils::getDistanceToEndOfLane(src_pose, lanes); @@ -129,8 +128,7 @@ double calc_ego_dist_to_lanes_start( return std::numeric_limits::max(); } - const auto path = - route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr->current_lanes_path; if (path.points.empty()) { return std::numeric_limits::max(); @@ -142,44 +140,6 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel); - // const auto min_lat_acc = std::get<0>(min_max_lat_acc); - const auto max_lat_acc = std::get<1>(min_max_lat_acc); - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - - const auto calc_sum = [&](double sum, double shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); - return sum + (min_vel * t + finish_judge_buffer); - }; - - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); -} - -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction) -{ - if (!route_handler) { - return std::numeric_limits::max(); - } - - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); - return calc_minimum_lane_change_length(lane_change_parameters, shift_intervals); -} - double calc_maximum_lane_change_length( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) @@ -231,4 +191,244 @@ double calc_maximum_lane_change_length( return calc_maximum_lane_change_length( vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); } + +std::vector calc_all_min_lc_lengths( + const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return {}; + } + + const auto min_vel = lc_param_ptr->minimum_lane_changing_velocity; + const auto min_max_lat_acc = lc_param_ptr->lane_change_lat_acc_map.find(min_vel); + const auto max_lat_acc = std::get<1>(min_max_lat_acc); + const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + + std::vector min_lc_lengths{}; + min_lc_lengths.reserve(shift_intervals.size()); + + const auto min_lc_length = [&](const auto shift_interval) { + const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + return min_vel * t; + }; + + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(min_lc_lengths), + min_lc_length); + + return min_lc_lengths; +} + +std::vector calc_all_max_lc_lengths( + const CommonDataPtr & common_data_ptr, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return {}; + } + + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; + const auto max_acc = common_data_ptr->transient_data.acc.max; + + const auto limit_vel = [&](const auto vel) { + const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; + return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + }; + + auto vel = limit_vel(common_data_ptr->get_ego_speed()); + + std::vector max_lc_lengths{}; + + const auto max_lc_length = [&](const auto shift_interval) { + // prepare section + vel = limit_vel(vel + max_acc * t_prepare); + const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + + // lane changing section + const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); + const auto t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto lane_changing_length = + vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; + + vel = limit_vel(vel + max_acc * t_lane_changing); + return prepare_length + lane_changing_length; + }; + + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(max_lc_lengths), + max_lc_length); + return max_lc_lengths; +} + +double calc_distance_buffer( + const LCParamPtr & lc_param_ptr, const std::vector & min_lc_lengths) +{ + if (min_lc_lengths.empty()) { + return std::numeric_limits::max(); + } + + const auto finish_judge_buffer = lc_param_ptr->lane_change_finish_judge_buffer; + const auto backward_buffer = calc_stopping_distance(lc_param_ptr); + const auto lengths_sum = std::accumulate(min_lc_lengths.begin(), min_lc_lengths.end(), 0.0); + const auto num_of_lane_changes = static_cast(min_lc_lengths.size()); + return lengths_sum + (num_of_lane_changes * finish_judge_buffer) + + ((num_of_lane_changes - 1.0) * backward_buffer); +} + +std::vector calc_shift_intervals( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto direction = common_data_ptr->direction; + + return route_handler_ptr->getLateralIntervalsToPreferredLane(lanes.back(), direction); +} + +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + const auto shift_intervals = calculation::calc_shift_intervals(common_data_ptr, lanes); + const auto all_min_lc_lengths = + calculation::calc_all_min_lc_lengths(common_data_ptr->lc_param_ptr, shift_intervals); + const auto min_lc_length = + !all_min_lc_lengths.empty() ? all_min_lc_lengths.front() : std::numeric_limits::max(); + const auto min_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_min_lc_lengths); + + const auto all_max_lc_lengths = + calculation::calc_all_max_lc_lengths(common_data_ptr, shift_intervals); + const auto max_lc_length = + !all_max_lc_lengths.empty() ? all_max_lc_lengths.front() : std::numeric_limits::max(); + const auto max_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_max_lc_lengths); + + return {{min_lc_length, max_lc_length}, {min_dist_buffer, max_dist_buffer}}; +} + +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} + +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + return 0.0; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); +} + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, + const std::vector & lon_accel_values, const double current_velocity, + const double min_length_threshold, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + std::vector metrics; + + auto is_skip = [&](const double prepare_length) { + if (prepare_length > max_length_threshold || prepare_length < min_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: prepare length out of expected range. length: %.5f, threshold min: %.5f, max: %.5f", + prepare_length, min_length_threshold, max_length_threshold); + return true; + } + return false; + }; + + metrics.reserve(prepare_durations.size() * lon_accel_values.size()); + for (const auto & prepare_duration : prepare_durations) { + for (const auto & lon_accel : lon_accel_values) { + const auto prepare_velocity = + std::clamp(current_velocity + lon_accel * prepare_duration, min_lc_vel, max_vel); + + // compute actual longitudinal acceleration + const double prepare_accel = (prepare_duration < 1e-3) + ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); + + const auto prepare_length = + calc_phase_length(current_velocity, max_vel, prepare_accel, prepare_duration); + + if (is_skip(prepare_length)) continue; + + metrics.emplace_back( + prepare_duration, prepare_length, prepare_velocity, lon_accel, prepare_accel, 0.0); + } + } + + return metrics; +} + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_path_velocity, const double lon_accel, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_data_ptr->lc_param_ptr->lane_change_lat_acc_map.find(initial_velocity); + const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / + common_data_ptr->lc_param_ptr->lateral_acc_sampling_num; + + std::vector metrics; + + auto is_skip = [&](const double lane_changing_length) { + if (lane_changing_length > max_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: lane changing length exceeds maximum threshold. length: %.5f, threshold: %.5f", + lane_changing_length, max_length_threshold); + return true; + } + return false; + }; + + for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; + lat_acc += lateral_acc_resolution) { + const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); + + const double lane_changing_accel = calc_lane_changing_acceleration( + initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + + const auto lane_changing_length = calculation::calc_phase_length( + initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); + + if (is_skip(lane_changing_length)) continue; + + const auto lane_changing_velocity = std::clamp( + initial_velocity + lane_changing_accel * lane_changing_duration, min_lc_vel, max_vel); + + metrics.emplace_back( + lane_changing_duration, lane_changing_length, lane_changing_velocity, lon_accel, + lane_changing_accel, lat_acc); + } + + return metrics; +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 6ff69abb85d18..8794d79a4d10f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -74,7 +74,8 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { constexpr const char * name{"lane_change.utils"}; - return rclcpp::get_logger(name); + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; } double calcLaneChangeResampleInterval( @@ -105,19 +106,6 @@ double calcMaximumAcceleration( return std::clamp(acc, 0.0, max_longitudinal_acc); } -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) -{ - if (prepare_longitudinal_acc <= 0.0) { - return 0.0; - } - - return std::clamp( - (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - prepare_longitudinal_acc); -} - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { @@ -144,7 +132,7 @@ std::vector getAccelerationValues( } if (max_acc - min_acc < std::numeric_limits::epsilon()) { - return {0.0}; + return {min_acc}; } constexpr double epsilon = 0.001; @@ -212,10 +200,39 @@ bool isPathInLanelets( return true; } -std::optional constructCandidatePath( +bool path_footprint_exceeds_target_lane_bound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin) +{ + if (common_data_ptr->direction == Direction::NONE || path.points.empty()) { + return false; + } + + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const bool is_left = common_data_ptr->direction == Direction::LEFT; + + const auto combined_target_lane = lanelet::utils::combineLaneletsShape(target_lanes); + + for (const auto & path_point : path.points) { + const auto & pose = path_point.point.pose; + const auto front_vertex = getEgoFrontVertex(pose, ego_info, is_left); + + const auto sign = is_left ? -1.0 : 1.0; + const auto dist_to_boundary = + sign * utils::getSignedDistanceFromLaneBoundary(combined_target_lane, front_vertex, is_left); + + if (dist_to_boundary < margin) { + RCLCPP_DEBUG(get_logger(), "Path footprint exceeds target lane boundary"); + return true; + } + } + + return false; +} + +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; @@ -267,8 +284,8 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - autoware::motion_utils::findNearestIndex(target_segment.points, point.point.pose); - point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; } // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster @@ -338,18 +355,7 @@ PathWithLaneId getReferencePathFromTargetLane( lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); } -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length) -{ - const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; - - return getLaneChangingShiftLine( - lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); -} - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length) { @@ -542,31 +548,6 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction) -{ - if (current_lanes.empty()) { - return false; - } - - const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), lane_change_parameters, direction); - 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; - } - - if ( - abort_plus_lane_change_length > - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) { - return false; - } - - return true; -} - std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) @@ -730,12 +711,12 @@ bool isParkedObject( using lanelet::geometry::toArcCoordinates; const double object_vel_norm = - std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { return false; } - const auto & object_pose = object.initial_pose.pose; + const auto & object_pose = object.initial_pose; const auto object_closest_index = autoware::motion_utils::findNearestIndex(path.points, object_pose.position); const auto object_closest_pose = path.points.at(object_closest_index).point.pose; @@ -802,7 +783,7 @@ bool isParkedObject( { using lanelet::geometry::distance2d; - const auto & obj_pose = object.initial_pose.pose; + const auto & obj_pose = object.initial_pose; const auto & obj_shape = object.shape; const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; @@ -845,9 +826,7 @@ bool passed_parked_objects( lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto current_lane_path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & current_lane_path = common_data_ptr->current_lanes_path; if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { return true; @@ -863,7 +842,7 @@ bool passed_parked_objects( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return true; } @@ -895,7 +874,7 @@ bool passed_parked_objects( const auto current_pose = common_data_ptr->get_ego_pose(); const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { return true; @@ -924,12 +903,11 @@ std::optional getLeadingStaticObjectIdx( std::optional leading_obj_idx = std::nullopt; for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose.pose; + const auto & obj_pose = obj.initial_pose; // ignore non-static object // TODO(shimizu): parametrize threshold - const double obj_vel_norm = - std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); if (obj_vel_norm > 1.0) { continue; } @@ -985,8 +963,8 @@ ExtendedPredictedObject transform( const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; - const double obj_vel_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); + const double obj_vel_norm = + std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { @@ -1048,6 +1026,14 @@ Polygon2d getEgoCurrentFootprint( return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } +Point getEgoFrontVertex( + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info, bool left) +{ + const double lon_offset = ego_info.wheel_base_m + ego_info.front_overhang_m; + const double lat_offset = 0.5 * (left ? ego_info.vehicle_width_m : -ego_info.vehicle_width_m); + return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; +} + bool isWithinIntersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) @@ -1075,16 +1061,6 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); } -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double duration) -{ - const auto length_with_acceleration = - velocity * duration + 0.5 * acceleration * std::pow(duration, 2); - const auto length_with_max_velocity = maximum_velocity * duration; - return std::min(length_with_acceleration, length_with_max_velocity); -} - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { const auto & lanes = common_data_ptr->lanes_ptr; @@ -1140,16 +1116,6 @@ bool is_same_lane_with_prev_iteration( (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation) -{ - Pose pose; - pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - pose.orientation = orientation; - return pose; -} - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object) @@ -1274,47 +1240,3 @@ double get_distance_to_next_regulatory_element( return distance; } } // namespace autoware::behavior_path_planner::utils::lane_change - -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Point32 p; - p.x = static_cast(pose.position.x); - p.y = static_cast(pose.position.y); - p.z = static_cast(pose.position.z); - return p; -}; - -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset) -{ - 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; - - // if stationary object, extend forward and backward by the half of lon length - const double forward_lon_offset = base_to_front + additional_lon_offset; - const double backward_lon_offset = -base_to_rear; - const double lat_offset = width / 2.0 + additional_lat_offset; - - const auto p1 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); - geometry_msgs::msg::Polygon polygon; - - polygon.points.push_back(create_point32(p1)); - polygon.points.push_back(create_point32(p2)); - polygon.points.push_back(create_point32(p3)); - polygon.points.push_back(create_point32(p4)); - - return polygon; -} - -} // namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 981051a363029..6196a4f02cff9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -40,6 +40,7 @@ autoware_behavior_path_planner_common autoware_freespace_planning_algorithms autoware_frenet_planner + autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils @@ -47,12 +48,12 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs behaviortree_cpp_v3 geometry_msgs - interpolation libboost-dev libopencv-dev magic_enum @@ -61,7 +62,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 9296fd22d374b..f341fb6ba90b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include @@ -108,7 +108,8 @@ inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const /// @return point interpolated between a and b as per the given ratio inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { - return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; + return { + interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)}; } /// @brief calculate the point with distance and arc length relative to a linestring diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 0e7d1cee65f02..795dc6d145190 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include +#include #include #include @@ -23,7 +24,7 @@ #include -#include +#include #include #include #include @@ -33,7 +34,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -60,8 +63,8 @@ struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped Polygon2d poly; PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + const double time, const Pose & pose, const double velocity, Polygon2d poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(std::move(poly)) { } }; @@ -75,22 +78,30 @@ struct PredictedPathWithPolygon struct ExtendedPredictedObject { unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_perception_msgs::msg::Shape shape; - std::vector classification; + Pose initial_pose; + Twist initial_twist; + Shape shape; + ObjectClassification classification; + Polygon2d initial_polygon; std::vector predicted_paths; + double dist_from_ego{0.0}; ///< Distance from ego to obj, can be arc length or euclidean. ExtendedPredictedObject() = default; explicit ExtendedPredictedObject(const PredictedObject & object) : uuid(object.object_id), - initial_pose(object.kinematics.initial_pose_with_covariance), - initial_twist(object.kinematics.initial_twist_with_covariance), - initial_acceleration(object.kinematics.initial_acceleration_with_covariance), - shape(object.shape), - classification(object.classification) + initial_pose(object.kinematics.initial_pose_with_covariance.pose), + initial_twist(object.kinematics.initial_twist_with_covariance.twist), + shape(object.shape) { + classification.label = std::invoke([&]() { + auto max_elem = std::max_element( + object.classification.begin(), object.classification.end(), + [](const auto & a, const auto & b) { return a.probability < b.probability; }); + + return (max_elem != object.classification.end()) ? max_elem->label + : ObjectClassification::UNKNOWN; + }); + initial_polygon = autoware::universe_utils::toPolygon2d(initial_pose, shape); } }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 7a189250c088d..b1d3a39379d15 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -44,6 +44,7 @@ autoware_adapi_v1_msgs autoware_freespace_planning_algorithms + autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils @@ -55,7 +56,6 @@ autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation magic_enum object_recognition_utils rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index d57c2339041f4..90693aa65e661 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -608,7 +608,7 @@ MarkerArray showFilteredObjects( cube_marker = default_cube_marker(1.0, 1.0, color); cube_marker.pose = pose; }; - insert_cube_marker(obj.initial_pose.pose); + insert_cube_marker(obj.initial_pose); }); return marker_array; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 0e005166a97c0..22d80da2cfa67 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -138,11 +138,12 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold) { - const auto requires_turn_signal = [&](const auto & lane_attribute) { + const auto requires_turn_signal = [¤t_vel]( + const auto & turn_direction, const bool is_in_turn_lane) { constexpr double stop_velocity_threshold = 0.1; return ( - lane_attribute == "right" || lane_attribute == "left" || - (lane_attribute == "straight" && current_vel < stop_velocity_threshold)); + turn_direction == "right" || turn_direction == "left" || + (turn_direction == "straight" && current_vel < stop_velocity_threshold && !is_in_turn_lane)); }; // base search distance const double base_search_distance = @@ -160,6 +161,19 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } } + bool is_in_turn_lane = false; + for (const auto & lane_id : unique_lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + if (turn_direction == "left" || turn_direction == "right") { + const auto & position = current_pose.position; + const lanelet::BasicPoint2d point(position.x, position.y); + if (lanelet::geometry::inside(lanelet, point)) { + is_in_turn_lane = true; + break; + } + } + } // combine consecutive lanes of the same turn direction // stores lanes that have already been combine std::set processed_lanes; @@ -175,7 +189,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( // Get the lane and its attribute const std::string lane_attribute = current_lane.attributeOr("turn_direction", std::string("none")); - if (!requires_turn_signal(lane_attribute)) continue; + if (!requires_turn_signal(lane_attribute, is_in_turn_lane)) continue; do { processed_lanes.insert(current_lane.id()); @@ -256,7 +270,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } else if (search_distance <= dist_to_front_point) { continue; } - if (requires_turn_signal(lane_attribute)) { + if (requires_turn_signal(lane_attribute, is_in_turn_lane)) { // update map if necessary if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { desired_start_point_map_.emplace(lane_id, current_pose); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index dcab7c22a35fe..648bb0a17622c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,13 +20,13 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include #include #include #include #include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index dc0f13270bca2..e2218e37a771b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" - -#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" - +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 1de55dca29347..25b307ab2cc4d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -327,7 +327,7 @@ ExtendedPredictedObject transform( { ExtendedPredictedObject extended_object(object); - const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto obj_velocity = extended_object.initial_twist.linear.x; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index fb1da48e41875..59fc726bd10a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -15,10 +15,10 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "interpolation/linear_interpolation.hpp" #include #include @@ -364,7 +364,7 @@ std::optional calcInterpolatedPoseWithVelocity( const auto interpolated_pose = autoware::universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = - interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); + autoware::interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; } } @@ -593,7 +593,7 @@ std::vector getCollidedPolygons( { debug.ego_predicted_path = predicted_ego_path; debug.obj_predicted_path = target_object_path.path; - debug.current_obj_pose = target_object.initial_pose.pose; + debug.current_obj_pose = target_object.initial_pose; } std::vector collided_polygons{}; @@ -709,11 +709,10 @@ bool checkPolygonsIntersects( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = - autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.current_obj_pose = obj.initial_pose; + debug.extended_obj_polygon = autoware::universe_utils::toPolygon2d(obj.initial_pose, obj.shape); debug.obj_shape = obj.shape; - debug.current_twist = obj.initial_twist.twist; + debug.current_twist = obj.initial_twist; return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 8a6c899b4bd6d..69c538fa6cb54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include -#include #include #include @@ -252,7 +252,7 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs query_distance.push_back(dist_from_start); } if (!query_distance.empty()) { - query_length = interpolation::spline(base_distance, base_length, query_distance); + query_length = autoware::interpolation::spline(base_distance, base_length, query_distance); } // Apply shifting. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 5b234198d9137..3b06e084690e3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -17,12 +17,12 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index 9ede8342856a6..5f9f4bcd12b75 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -16,6 +16,7 @@ autoware_behavior_path_planner_common autoware_bezier_sampler autoware_frenet_planner + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_path_sampler @@ -23,16 +24,15 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs - interpolation pluginlib rclcpp rclcpp_components sensor_msgs - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 8fdedf38ed756..f05045f284413 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -956,7 +956,7 @@ autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSampl 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)); + target_lateral_positions.push_back(autoware::interpolation::lerp(min_d, max_d, r)); } } else { target_lateral_positions = params_.sampling.target_lateral_positions; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 4b3a76c8530dd..9df0791ed9375 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -559,7 +559,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, ego_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_pose.position, o.initial_pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; arc_length_to_closet_object = arc_length; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 35ee73ca8423e..c8236c4e50cd8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -73,8 +73,6 @@ struct ObjectParameter bool is_safety_check_target{false}; - size_t execute_num{1}; - double moving_speed_threshold{0.0}; double moving_time_threshold{1.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index f997b30da9ffa..328ac199aca1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -29,6 +29,7 @@ autoware_perception_msgs autoware_route_handler autoware_rtc_interface + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils geometry_msgs @@ -36,7 +37,6 @@ pluginlib rclcpp sensor_msgs - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index e3ecdff3759c0..4d13d4164d263 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -838,20 +838,20 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); - const auto & object_twist = object.initial_twist.twist; + const auto & object_twist = object.initial_twist; const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); - const auto object_type = utils::getHighestProbLabel(object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto object_type = object.classification; + const auto object_parameter = parameters_->object_parameters.at(object_type.label); const auto is_object_moving = v_norm > object_parameter.moving_speed_threshold; const auto is_object_oncoming = is_object_moving && - utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); + utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index a7166b52fc4e5..adea18c726ced 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -256,8 +256,6 @@ bool isWithinIntersection( return false; } - const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); - const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); @@ -752,8 +750,8 @@ bool isObviousAvoidanceTarget( } bool isSatisfiedWithCommonCondition( - ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, - const std::shared_ptr & planner_data, + ObjectData & object, const PathWithLaneId & path, const double forward_detection_range, + const double to_goal_distance, const Point & ego_pos, const bool is_allowed_goal_modification, const std::shared_ptr & parameters) { // Step1. filtered by target object type. @@ -769,8 +767,7 @@ bool isSatisfiedWithCommonCondition( } // Step3. filtered by longitudinal distance. - const auto & ego_pos = planner_data->self_odometry->pose.pose.position; - fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object); + fillLongitudinalAndLengthByClosestEnvelopeFootprint(path, ego_pos, object); if (object.longitudinal < -parameters->object_check_backward_distance) { object.info = ObjectInfo::FURTHER_THAN_THRESHOLD; @@ -785,20 +782,12 @@ bool isSatisfiedWithCommonCondition( // Step4. filtered by distance between object and goal position. // TODO(Satoshi OTA): remove following two conditions after it can execute avoidance and goal // planner module simultaneously. - const auto & rh = planner_data->route_handler; - const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); - const auto to_goal_distance = - rh->isInGoalRouteSection(data.current_lanelets.back()) - ? autoware::motion_utils::calcSignedArcLength( - data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) - : std::numeric_limits::max(); - if (object.longitudinal > to_goal_distance) { object.info = ObjectInfo::FURTHER_THAN_GOAL; return false; } - if (!utils::isAllowedGoalModification(planner_data->route_handler)) { + if (!is_allowed_goal_modification) { if ( object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > to_goal_distance) { @@ -1824,6 +1813,9 @@ void updateRoadShoulderDistance( clip_objects.push_back(object); } }); + + if (clip_objects.empty()) return; + for (auto & o : clip_objects) { const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(o.object.classification); @@ -1866,9 +1858,21 @@ void filterTargetObjects( data.target_objects.push_back(object); }; + const auto & rh = planner_data->route_handler; + const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); + const auto to_goal_distance = + rh->isInGoalRouteSection(data.current_lanelets.back()) + ? autoware::motion_utils::calcSignedArcLength( + data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) + : std::numeric_limits::max(); + const auto & is_allowed_goal_modification = + utils::isAllowedGoalModification(planner_data->route_handler); + for (auto & o : objects) { if (!filtering_utils::isSatisfiedWithCommonCondition( - o, data, forward_detection_range, planner_data, parameters)) { + o, data.reference_path_rough, forward_detection_range, to_goal_distance, + planner_data->self_odometry->pose.pose.position, is_allowed_goal_modification, + parameters)) { data.other_objects.push_back(o); continue; } @@ -1888,6 +1892,7 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); } else if (filtering_utils::isVehicleTypeObject(o)) { // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index cd9c63fc0367f..be04f1f9180b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -34,7 +35,6 @@ geometry_msgs grid_map_core grid_map_ros - interpolation libboost-dev nav_msgs pcl_conversions diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index d7e8dbda57f3b..f56552485b5fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -332,7 +332,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double dist_default_stop = default_stop_pose_opt.has_value() ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position) - : std::numeric_limits::max(); + : 0.0; updateObjectState( dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 93e1976a251ba..1c5d23e31b2e1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -31,7 +32,6 @@ autoware_vehicle_info_utils fmt geometry_msgs - interpolation libopencv-dev magic_enum nav_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 1ae99cafaceb1..609ff010d20e0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -441,6 +441,7 @@ void IntersectionModuleManager::setActivation() scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); intersection_module->setOcclusionActivation( occlusion_rtc_interface_.isActivated(occlusion_uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(getUUID(scene_module->getModuleId()))); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index ec6610048dc79..54af88c2f0fbb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -692,10 +692,11 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( object_info->predicted_object()); continue; } - if (!object_info->unsafe_info()) { + const auto unsafe_info_opt = object_info->unsafe_info(); + if (!unsafe_info_opt) { continue; } - const auto & unsafe_info = object_info->unsafe_info().value(); + const auto & unsafe_info = unsafe_info_opt.value(); // ========================================================================================== // if ego is over the pass judge lines, then the visualization as "too_late_objects" or // "misjudge_objects" is more important than that for "unsafe" diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index ae1d9768eaecc..b694c8aaa2e3e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -17,11 +17,11 @@ #include // for to_bg2d #include // for planning_utils:: +#include #include #include // for lanelet::autoware::RoadMarking #include #include -#include #include #include @@ -146,7 +146,7 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) points.push_back(*point); } - SplineInterpolationPoints2d interpolation(points); + autoware::interpolation::SplineInterpolationPoints2d interpolation(points); const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); std::vector curvatures_positive; for (const auto & curvature : curvatures) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index b630f988af662..21664f7596d60 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_vehicle_info_utils eigen geometry_msgs - interpolation libboost-dev pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index f2bcc0b5abf44..ac58c62036e26 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -17,10 +17,10 @@ #include #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index 22da467ba9fac..b414769e84ec2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -18,6 +18,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_vehicle_info_utils geometry_msgs grid_map_ros - interpolation libboost-dev libopencv-dev nav_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 6d40c2167961a..56a5acd8c7dc4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -18,10 +18,10 @@ #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 6e7f8b679d32a..4075f356c187a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -51,7 +51,8 @@ class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) + : clock_(node.get_clock()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { max_stop_acceleration_threshold = node.declare_parameter( "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? @@ -60,6 +61,8 @@ struct PlannerData delay_response_time = node.declare_parameter("delay_response_time"); } + rclcpp::Clock::SharedPtr clock_; + // msgs from callbacks that are used for data-ready geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; @@ -108,7 +111,7 @@ struct PlannerData } // Get velocities within stop_duration - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + const auto now = clock_->now(); std::vector vs; for (const auto & velocity : velocity_buffer) { vs.push_back(velocity.twist.linear.x); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 0122c220e7e51..9e5e8b0edda7b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -34,7 +35,6 @@ diagnostic_msgs eigen geometry_msgs - interpolation nav_msgs pcl_conversions rclcpp diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md index 0b6aa697fcbef..dfc88ef676838 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -27,18 +27,18 @@ The length of the trajectory used for generating the footprints is limited by th ![ego_footprints](./docs/footprints.png) -### 2. Ego lanelets +### 2. Other lanelets -In the second step, we calculate the lanelets followed by the ego trajectory. -We select all lanelets crossed by the trajectory linestring (sequence of trajectory points), as well as their preceding lanelets. +In the second step, we calculate the lanelets where collisions should be avoided. +We consider all lanelets around the ego vehicle that are not crossed by the trajectory linestring (sequence of trajectory points) or their preceding lanelets. -![ego_lane](./docs/ego_lane.png) +![other_lanes](./docs/other_lanes.png) -In the debug visualization the combination of all ego lanelets is shown as a blue polygon. +In the debug visualization, these other lanelets are shown as blue polygons. ### 3. Out of lane areas -Next, for each trajectory point, we create the corresponding out of lane areas by subtracting the ego lanelets (from step 2) from the trajectory point footprint (from step 1). +Next, for each trajectory point, we create the corresponding out of lane areas by intersection the other lanelets (from step 2) with the trajectory point footprint (from step 1). Each area is associated with the lanelets overlapped by the area and with the corresponding ego trajectory point. ![out_of_lane_areas](./docs/out_of_lane_areas.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png deleted file mode 100644 index 65cb73226465a..0000000000000 Binary files a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png and /dev/null differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png new file mode 100644 index 0000000000000..a56d6427f59e3 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 3336278c761c6..e0fb81e2d7a5a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -10,6 +10,7 @@ Shumpei Wakabayashi Takayuki Murooka Mamoru Sobue + Alqudah Mohammad Apache License 2.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index b7cd55ce2e642..4edeb275457b1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -21,8 +21,7 @@ #include #include -#include -#include +#include #include #include @@ -78,7 +77,7 @@ std::optional calculate_pose_ahead_of_collision( const auto interpolated_pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); - if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + if (!boost::geometry::disjoint(interpolated_footprint, point_to_avoid.out_overlaps)) { return interpolated_pose; } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 6febd15ef8052..7d845eb788ef8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -21,11 +21,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include #include #include @@ -36,6 +37,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -151,53 +153,59 @@ size_t add_stop_line_markers( } return max_id; } -} // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array( - const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, - const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +void add_out_lanelets( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const lanelet::ConstLanelets & out_lanelets) { - const auto z = ego_data.pose.position.z; - visualization_msgs::msg::MarkerArray debug_marker_array; - - auto base_marker = get_base_marker(); - base_marker.pose.position.z = z + 0.5; - base_marker.ns = "footprints"; - base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); - // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation - // disabled to prevent performance issues when publishing the debug markers - // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); - lanelet::BasicPolygons2d drivable_lane_polygons; - for (const auto & poly : ego_data.drivable_lane_polygons) { - drivable_lane_polygons.push_back(poly.outer); + for (const auto & ll : out_lanelets) { + drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon()); } - base_marker.ns = "ego_lane"; + base_marker.ns = "out_lanelets"; base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons); + add_polygons_markers(marker_array, base_marker, drivable_lane_polygons); +} - lanelet::BasicPolygons2d out_of_lane_areas; - for (const auto & p : out_of_lane_data.outside_points) { - out_of_lane_areas.push_back(p.outside_ring); +void add_out_of_lane_overlaps( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const std::vector & outside_points, + const std::vector & trajectory) +{ + lanelet::BasicPolygons2d out_of_lane_overlaps; + lanelet::BasicPolygon2d out_of_lane_overlap; + for (const auto & p : outside_points) { + for (const auto & overlap : p.out_overlaps) { + boost::geometry::convert(overlap, out_of_lane_overlap); + out_of_lane_overlaps.push_back(out_of_lane_overlap); + } } base_marker.ns = "out_of_lane_areas"; base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas); - for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) { - const auto & a = out_of_lane_data.outside_points[i]; - debug_marker_array.markers.back().points.push_back( - ego_data.trajectory_points[a.trajectory_index].pose.position); - const auto centroid = boost::geometry::return_centroid(a.outside_ring); - debug_marker_array.markers.back().points.push_back( - geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps); + for (const auto & p : outside_points) { + for (const auto & a : p.out_overlaps) { + marker_array.markers.back().points.push_back(trajectory[p.trajectory_index].pose.position); + const auto centroid = boost::geometry::return_centroid(a); + marker_array.markers.back().points.push_back( + geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + } } - +} +void add_predicted_paths( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const autoware_perception_msgs::msg::PredictedObjects & objects, + const geometry_msgs::msg::Pose & ego_pose) +{ + base_marker.ns = "objects"; + base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); lanelet::BasicPolygons2d object_polygons; + constexpr double max_draw_distance = 50.0; for (const auto & o : objects.objects) { for (const auto & path : o.kinematics.predicted_paths) { for (const auto & pose : path.path) { // limit the draw distance to improve performance - if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) { const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); object_polygons.push_back(ll_poly); @@ -205,9 +213,28 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array( } } } - base_marker.ns = "objects"; - base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, object_polygons); + add_polygons_markers(marker_array, base_marker, object_polygons); +} +} // namespace + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +{ + const auto z = ego_data.pose.position.z; + visualization_msgs::msg::MarkerArray debug_marker_array; + + auto base_marker = get_base_marker(); + base_marker.pose.position.z = z + 0.5; + base_marker.ns = "footprints"; + base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); + // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation + // disabled to prevent performance issues when publishing the debug markers + // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); + add_out_lanelets(debug_marker_array, base_marker, ego_data.out_lanelets); + add_out_of_lane_overlaps( + debug_marker_array, base_marker, out_of_lane_data.outside_points, ego_data.trajectory_points); + add_predicted_paths(debug_marker_array, base_marker, objects, ego_data.pose); add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index 564bf5de7ef2e..6e31f8c8455fd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -18,6 +18,8 @@ #include +#include + #include #include @@ -37,6 +39,7 @@ universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool {p.front_offset + front_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + boost::geometry::correct(base_footprint); return base_footprint; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index f520a564519f0..e4840d724e43d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -16,16 +16,24 @@ #include "types.hpp" +#include +#include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane { @@ -77,15 +85,13 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler) + const universe_utils::LineString2d & trajectory_ls, + const route_handler::RouteHandler & route_handler) { const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; - lanelet::BasicLineString2d trajectory_ls; - for (const auto & p : ego_data.trajectory_points) - trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); - const auto candidates = - lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); + const auto candidates = lanelet_map_ptr->laneletLayer.search( + boost::geometry::return_envelope(trajectory_ls)); for (const auto & ll : candidates) { if ( is_road_lanelet(ll) && @@ -115,51 +121,77 @@ lanelet::ConstLanelets calculate_ignored_lanelets( return ignored_lanelets; } -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler) +lanelet::ConstLanelets calculate_out_lanelets( + const lanelet::LaneletLayer & lanelet_layer, + const universe_utils::MultiPolygon2d & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets) { - const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); - const auto ignored_lanelets = - out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); - for (const auto & ll : route_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; - } - for (const auto & ll : ignored_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; + lanelet::ConstLanelets out_lanelets; + const auto candidates = lanelet_layer.search( + boost::geometry::return_envelope(trajectory_footprints)); + for (const auto & lanelet : candidates) { + const auto id = lanelet.id(); + if ( + contains_lanelet(trajectory_lanelets, id) || contains_lanelet(ignored_lanelets, id) || + !is_road_lanelet(lanelet)) { + continue; + } + if (!boost::geometry::disjoint(trajectory_footprints, lanelet.polygon2d().basicPolygon())) { + out_lanelets.push_back(lanelet); + } } + return out_lanelets; } -void calculate_overlapped_lanelets( - OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) +OutLaneletRtree calculate_out_lanelet_rtree(const lanelet::ConstLanelets & lanelets) { - out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); - const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( - lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); - for (const auto & ll : candidates) { - if ( - is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && - boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { - out_of_lane_point.overlapped_lanelets.push_back(ll); - } + std::vector nodes; + nodes.reserve(lanelets.size()); + for (auto i = 0UL; i < lanelets.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope( + lanelets[i].polygon2d().basicPolygon()), + i); } + return {nodes.begin(), nodes.end()}; } -void calculate_overlapped_lanelets( - OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params) { - for (auto it = out_of_lane_data.outside_points.begin(); - it != out_of_lane_data.outside_points.end();) { - calculate_overlapped_lanelets(*it, route_handler); - if (it->overlapped_lanelets.empty()) { - // do not keep out of lane points that do not overlap any lanelet - out_of_lane_data.outside_points.erase(it); - } else { - ++it; - } + universe_utils::LineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) { + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); } + // add a point beyond the last trajectory point to account for the ego front offset + const auto pose_beyond = universe_utils::calcOffsetPose( + ego_data.trajectory_points.back().pose, params.front_offset, 0.0, 0.0, 0.0); + trajectory_ls.emplace_back(pose_beyond.position.x, pose_beyond.position.y); + const auto trajectory_lanelets = calculate_trajectory_lanelets(trajectory_ls, route_handler); + const auto ignored_lanelets = calculate_ignored_lanelets(trajectory_lanelets, route_handler); + + const auto max_ego_footprint_offset = std::max({ + params.front_offset + params.extra_front_offset, + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset, + params.rear_offset + params.extra_rear_offset, + }); + universe_utils::MultiPolygon2d trajectory_footprints; + const boost::geometry::strategy::buffer::distance_symmetric distance_strategy( + max_ego_footprint_offset); + const boost::geometry::strategy::buffer::join_miter join_strategy; + const boost::geometry::strategy::buffer::end_flat end_strategy; + const boost::geometry::strategy::buffer::point_square circle_strategy; + const boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::buffer( + trajectory_ls, trajectory_footprints, distance_strategy, side_strategy, join_strategy, + end_strategy, circle_strategy); + + ego_data.out_lanelets = calculate_out_lanelets( + route_handler.getLaneletMapPtr()->laneletLayer, trajectory_footprints, trajectory_lanelets, + ignored_lanelets); + ego_data.out_lanelets_rtree = calculate_out_lanelet_rtree(ego_data.out_lanelets); } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index a7729deb539b6..0cb9e223c6456 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include @@ -43,7 +44,8 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @param [in] route_handler route handler /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler); + const universe_utils::LineString2d & trajectory_ls, + const std::shared_ptr route_handler); /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during /// a lane change @@ -66,14 +68,18 @@ lanelet::ConstLanelets calculate_ignored_lanelets( /// @brief calculate the polygons representing the ego lane and add it to the ego data /// @param [inout] ego_data ego data /// @param [in] route_handler route handler with map information -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler); +// void calculate_drivable_lane_polygons( +// EgoData & ego_data, const route_handler::RouteHandler & route_handler); /// @brief calculate the lanelets overlapped at each out of lane point /// @param [out] out_of_lane_data data with the out of lane points /// @param [in] route_handler route handler with the lanelet map void calculate_overlapped_lanelets( OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); + +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp index aef035200b6cc..176b0d2aed2b3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -20,15 +20,15 @@ #include #include -#include +#include -#include -#include #include #include +#include #include #include +#include #include #include @@ -47,7 +47,7 @@ void update_collision_times( auto & out_of_lane_point = out_of_lane_data.outside_points[index]; if (out_of_lane_point.collision_times.count(time) == 0UL) { const auto has_collision = - !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + !boost::geometry::disjoint(out_of_lane_point.out_overlaps, object_footprint.outer()); if (has_collision) { out_of_lane_point.collision_times.insert(time); } @@ -129,20 +129,39 @@ void calculate_collisions_to_avoid( } } +OutOfLanePoint calculate_out_of_lane_point( + const lanelet::BasicPolygon2d & footprint, const lanelet::ConstLanelets & out_lanelets, + const OutLaneletRtree & out_lanelets_rtree) +{ + OutOfLanePoint p; + std::vector candidates; + out_lanelets_rtree.query( + boost::geometry::index::intersects(footprint), std::back_inserter(candidates)); + for (const auto & [_, idx] : candidates) { + const auto & lanelet = out_lanelets[idx]; + lanelet::BasicPolygons2d intersections; + boost::geometry::intersection(footprint, lanelet.polygon2d().basicPolygon(), intersections); + for (const auto & intersection : intersections) { + universe_utils::Polygon2d poly; + boost::geometry::convert(intersection, poly); + p.out_overlaps.push_back(poly); + } + if (!intersections.empty()) { + p.overlapped_lanelets.push_back(lanelet); + } + } + return p; +} std::vector calculate_out_of_lane_points(const EgoData & ego_data) { std::vector out_of_lane_points; - OutOfLanePoint p; for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - p.trajectory_index = i + ego_data.first_trajectory_idx; const auto & footprint = ego_data.trajectory_footprints[i]; - Polygons out_of_lane_polygons; - boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); - for (const auto & area : out_of_lane_polygons) { - if (!area.outer.empty()) { - p.outside_ring = area.outer; - out_of_lane_points.push_back(p); - } + OutOfLanePoint p = + calculate_out_of_lane_point(footprint, ego_data.out_lanelets, ego_data.out_lanelets_rtree); + p.trajectory_index = ego_data.first_trajectory_idx + i; + if (!p.overlapped_lanelets.empty()) { + out_of_lane_points.push_back(p); } } return out_of_lane_points; @@ -152,11 +171,12 @@ void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data) { std::vector rtree_nodes; for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { - OutAreaNode n; - n.first = boost::geometry::return_envelope( - out_of_lane_data.outside_points[i].outside_ring); - n.second = i; - rtree_nodes.push_back(n); + for (const auto & out_overlap : out_of_lane_data.outside_points[i].out_overlaps) { + OutAreaNode n; + n.first = boost::geometry::return_envelope(out_overlap); + n.second = i; + rtree_nodes.push_back(n); + } } out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp index 33f0842c56d36..f0e0360ef1c15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index c97e10cc8706e..347a138b651fa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -221,12 +221,10 @@ void prepare_stop_lines_rtree( ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } -out_of_lane::OutOfLaneData prepare_out_of_lane_data( - const out_of_lane::EgoData & ego_data, const route_handler::RouteHandler & route_handler) +out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData & ego_data) { out_of_lane::OutOfLaneData out_of_lane_data; out_of_lane_data.outside_points = out_of_lane::calculate_out_of_lane_points(ego_data); - out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, route_handler); out_of_lane::prepare_out_of_lane_areas_rtree(out_of_lane_data); return out_of_lane_data; } @@ -255,11 +253,11 @@ VelocityPlanningResult OutOfLaneModule::plan( const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); stopwatch.tic("calculate_lanelets"); - out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); + out_of_lane::calculate_out_lanelet_rtree(ego_data, *planner_data->route_handler, params_); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); stopwatch.tic("calculate_out_of_lane_areas"); - auto out_of_lane_data = prepare_out_of_lane_data(ego_data, *planner_data->route_handler); + auto out_of_lane_data = prepare_out_of_lane_data(ego_data); const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); stopwatch.tic("filter_predicted_objects"); @@ -275,9 +273,12 @@ VelocityPlanningResult OutOfLaneModule::plan( out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); const auto calculate_times_us = stopwatch.toc("calculate_times"); - if ( - params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && - !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + const auto is_already_overlapping = + params_.skip_if_already_overlapping && + std::find_if(ego_data.out_lanelets.begin(), ego_data.out_lanelets.end(), [&](const auto & ll) { + return !boost::geometry::disjoint(ll.polygon2d().basicPolygon(), ego_data.current_footprint); + }) != ego_data.out_lanelets.end(); + if (is_already_overlapping) { RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( ego_data, out_of_lane_data, objects, debug_data_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index c3714c5609135..edf4aff8bc85c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -88,6 +88,8 @@ using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; using OutAreaNode = std::pair; using OutAreaRtree = bgi::rtree>; +using LaneletNode = std::pair; +using OutLaneletRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData @@ -100,7 +102,8 @@ struct EgoData double min_slowdown_distance{}; double min_stop_arc_length{}; - Polygons drivable_lane_polygons; + lanelet::ConstLanelets out_lanelets; + OutLaneletRtree out_lanelets_rtree; lanelet::BasicPolygon2d current_footprint; std::vector trajectory_footprints; @@ -108,10 +111,11 @@ struct EgoData StopLinesRtree stop_lines_rtree; }; +/// @brief data related to an out of lane trajectory point struct OutOfLanePoint { size_t trajectory_index; - lanelet::BasicPolygon2d outside_ring; + universe_utils::MultiPolygon2d out_overlaps; std::set collision_times; std::optional min_object_arrival_time; std::optional max_object_arrival_time; @@ -119,6 +123,8 @@ struct OutOfLanePoint lanelet::ConstLanelets overlapped_lanelets; bool to_avoid = false; }; + +/// @brief data related to the out of lane points struct OutOfLaneData { std::vector outside_points; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 515a30348cbee..dfe15579eed2a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -6,6 +6,7 @@ Common functions and interfaces for motion_velocity_planner modules Maxime Clement + Alqudah Mohammad Apache License 2.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp index fd9a7d6e6649b..5988272b39ae1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -74,7 +74,7 @@ bool all_within(const MultiPoint2d & pts1, const MultiPoint2d & pts2) return true; } -TEST(TestCollisionChecker, Benchmark) +TEST(TestCollisionChecker, DISABLED_Benchmark) { constexpr auto nb_ego_footprints = 1000; constexpr auto nb_obstacles = 1000; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index c1e68a232c55d..7919917c2256a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -6,6 +6,7 @@ Node of the motion_velocity_planner Maxime Clement + Alqudah Mohammad Apache License 2.0 diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index a27dd1576c90d..65ee862592bef 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index 992ad95c72bb1..73a2b0c95493e 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" #include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 26f85631d569f..ca137a0d06a53 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -15,13 +15,13 @@ autoware_bezier_sampler autoware_frenet_planner + autoware_interpolation autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs rclcpp rclcpp_components diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 15d7e883a31d2..34905f79cd364 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -14,6 +14,7 @@ #include "autoware_path_sampler/node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware_path_sampler/path_generation.hpp" @@ -22,7 +23,6 @@ #include "autoware_path_sampler/utils/trajectory_utils.hpp" #include "autoware_sampler_common/constraints/hard_constraint.hpp" #include "autoware_sampler_common/constraints/soft_constraint.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 06e8ab9042207..127393abc5ac3 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -82,7 +82,7 @@ autoware::frenet_planner::SamplingParameters prepareSamplingParameters( 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)); + target_lateral_positions.push_back(autoware::interpolation::lerp(min_d, max_d, r)); } } else { target_lateral_positions = params.sampling.target_lateral_positions; diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index e8379091615f8..e6317cd21f17d 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -65,7 +65,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::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)) { diff --git a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index 29b3f5a8fc145..c2cf432229b1b 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -17,8 +17,8 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include #include -#include #include @@ -269,7 +269,7 @@ struct Trajectory : Path 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); + t.times = autoware::interpolation::lerp(lengths, times, t.lengths); std::vector xs; std::vector ys; xs.reserve(points.size()); @@ -278,16 +278,18 @@ struct Trajectory : Path 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); + const auto new_xs = autoware::interpolation::lerp(times, xs, t.times); + const auto new_ys = autoware::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.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); + t.jerks = autoware::interpolation::lerp(times, jerks, t.times); + t.yaws = autoware::interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = + autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = + autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; t.cost = cost; return t; @@ -305,7 +307,7 @@ struct Trajectory : Path 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); + t.lengths = autoware::interpolation::lerp(times, lengths, t.times); std::vector xs; std::vector ys; xs.reserve(points.size()); @@ -314,16 +316,18 @@ struct Trajectory : Path 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); + const auto new_xs = autoware::interpolation::lerp(times, xs, t.times); + const auto new_ys = autoware::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.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); + t.jerks = autoware::interpolation::lerp(times, jerks, t.times); + t.yaws = autoware::interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = + autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = + autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; t.cost = cost; return t; diff --git a/planning/sampling_based_planner/autoware_sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml index a1462131d62ad..d1f1e3946a0dd 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -11,7 +11,7 @@ autoware_cmake - interpolation + autoware_interpolation ament_cmake_ros ament_lint_auto diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 02ce2a0098220..94deffd5ae9d0 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -68,12 +68,12 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_node.cpp src/downsample_filter/random_downsample_filter_node.cpp - src/downsample_filter/approximate_downsample_filter_nodelet.cpp + src/downsample_filter/approximate_downsample_filter_node.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp - src/outlier_filter/ring_outlier_filter_nodelet.cpp + src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp - src/outlier_filter/dual_return_outlier_filter_nodelet.cpp + src/outlier_filter/dual_return_outlier_filter_node.cpp src/passthrough_filter/passthrough_filter_node.cpp src/passthrough_filter/passthrough_filter_uint16_node.cpp src/passthrough_filter/passthrough_uint16.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml new file mode 100644 index 0000000000000..239e47f09632b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 0.1 diff --git a/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..5454176d7f319 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.0 + min_azimuth_deg: 135.0 + max_azimuth_deg: 225.0 + max_distance: 12.0 + vertical_bins: 128 + max_azimuth_diff: 50.0 + weak_first_distance_ratio: 1.004 + general_distance_ratio: 1.03 + weak_first_local_noise_threshold: 2 + roi_mode: "Fixed_xyz_ROI" + visibility_error_threshold: 0.5 + visibility_warn_threshold: 0.7 diff --git a/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..76bf68958f504 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index c6a45f7212c46..92ca1d3b3081c 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -36,11 +36,7 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, #### Approximate Downsample Filter -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ---------------- | -| `voxel_size_x` | double | 0.3 | voxel size x [m] | -| `voxel_size_y` | double | 0.3 | voxel size y [m] | -| `voxel_size_z` | double | 0.1 | voxel size z [m] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json") }} ### Random Downsample Filter diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 6c9a7ed14c2eb..8f4da273861a3 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -50,25 +50,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Description | -| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `vertical_bins` | int | The number of vertical bin for visibility histogram | -| `max_azimuth_diff` | float | Threshold for ring_outlier_filter | -| `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | -| `general_distance_ratio` | double | Threshold for ring_outlier_filter | -| `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | -| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | -| `roi_mode` | string | The name of ROI mode for switching | -| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | -| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | -| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | -| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | -| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | -| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | -| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md index 7f2efe90a674f..95ea2ed8ba969 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -56,20 +56,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| 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` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | -| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | -| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | -| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | -| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | -| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | -| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json") }} | ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp similarity index 92% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp index 7f5d3f83d3a30..abef5acad59dc 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -48,8 +48,8 @@ * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -72,9 +72,9 @@ class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preproc rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -83,5 +83,5 @@ class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preproc } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp similarity index 95% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index b8aba769b17a5..ef33e88ef5c98 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 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,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -94,5 +94,5 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp similarity index 97% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index caedeac62b88a..cf396e3e3a4be 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 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,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/pointcloud_preprocessor/transform_info.hpp" @@ -108,4 +108,4 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml new file mode 100644 index 0000000000000..f4375e5a5cd2f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml deleted file mode 100644 index c65996fbcdc65..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..d0293ca140e4f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..68076f5c9c321 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json new file mode 100644 index 0000000000000..7b582aa5377b4 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json @@ -0,0 +1,46 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Approximate Downsample Filter Node", + "type": "object", + "definitions": { + "approximate_downsample_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "voxel size along the x-axis [m]", + "default": "0.3", + "minimum": 0.0 + }, + "voxel_size_y": { + "type": "number", + "description": "voxel size along the y-axis [m]", + "default": "0.3", + "minimum": 0.0 + }, + "voxel_size_z": { + "type": "number", + "description": "voxel size along the z-axis [m]", + "default": "0.1", + "minimum": 0.0 + } + }, + "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/approximate_downsample_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..baaa0fa1f4604 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json @@ -0,0 +1,146 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Dual Return Outlier Filter Node", + "type": "object", + "definitions": { + "dual_return_outlier_filter": { + "type": "object", + "properties": { + "x_max": { + "type": "number", + "description": "Maximum of x in meters for `Fixed_xyz_ROI` mode", + "default": "18.0" + }, + "x_min": { + "type": "number", + "description": "Minimum of x in meters for `Fixed_xyz_ROI` mode", + "default": "-12.0" + }, + "y_max": { + "type": "number", + "description": "Maximum of y in meters for `Fixed_xyz_ROI` mode", + "default": "2.0" + }, + "y_min": { + "type": "number", + "description": "Minimum of y in meters for `Fixed_xyz_ROI` mode", + "default": "-2.0" + }, + "z_max": { + "type": "number", + "description": "Maximum of z in meters for `Fixed_xyz_ROI` mode", + "default": "10.0" + }, + "z_min": { + "type": "number", + "description": "Minimum of z in meters for `Fixed_xyz_ROI` mode", + "default": "0.0" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "135.0", + "minimum": 0, + "maximum": 360 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "225.0", + "minimum": 0, + "maximum": 360 + }, + "max_distance": { + "type": "number", + "description": "The limit distance in meters for `Fixed_azimuth_ROI` mode", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bins for the visibility histogram", + "default": "128", + "minimum": 1 + }, + "max_azimuth_diff": { + "type": "number", + "description": "The azimuth difference threshold in degrees for ring_outlier_filter", + "default": "50.0", + "minimum": 0.0 + }, + "weak_first_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive weak point distances", + "default": "1.004", + "minimum": 0.0 + }, + "general_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive normal point distances", + "default": "1.03", + "minimum": 0.0 + }, + "weak_first_local_noise_threshold": { + "type": "integer", + "description": "If the number of outliers among weak points is less than the weak_first_local_noise_threshold in the (max_azimuth - min_azimuth) / horizontal_bins interval, all points within the interval will not be filtered out.", + "default": "2", + "minimum": 0 + }, + "roi_mode": { + "type": "string", + "description": "roi mode", + "default": "Fixed_xyz_ROI", + "enum": ["Fixed_xyz_ROI", "No_ROI", "Fixed_azimuth_ROI"] + }, + "visibility_error_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR", + "default": "0.5", + "minimum": 0.0, + "maximum": 1.0 + }, + "visibility_warn_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN", + "default": "0.7", + "minimum": 0.0, + "maximum": 1.0 + } + }, + "required": [ + "x_max", + "x_min", + "y_max", + "y_min", + "z_max", + "z_min", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "max_azimuth_diff", + "weak_first_distance_ratio", + "general_distance_ratio", + "weak_first_local_noise_threshold", + "roi_mode", + "visibility_error_threshold", + "visibility_warn_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/dual_return_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..1c0cb406584ac --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -0,0 +1,113 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ring Outlier Filter Node", + "type": "object", + "definitions": { + "ring_outlier_filter": { + "type": "object", + "properties": { + "distance_ratio": { + "type": "number", + "description": "distance_ratio", + "default": "1.03", + "minimum": 0.0 + }, + "object_length_threshold": { + "type": "number", + "description": "object_length_threshold", + "default": "0.1", + "minimum": 0.0 + }, + "num_points_threshold": { + "type": "integer", + "description": "num_points_threshold", + "default": "4", + "minimum": 0 + }, + "max_rings_num": { + "type": "integer", + "description": "max_rings_num", + "default": "128", + "minimum": 1 + }, + "max_points_num_per_ring": { + "type": "integer", + "description": "Set this value large enough such that HFoV / resolution < max_points_num_per_ring", + "default": "4000", + "minimum": 0 + }, + "publish_outlier_pointcloud": { + "type": "boolean", + "description": "Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments.", + "default": "false" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth for visibility score calculation", + "default": "0.0", + "minimum": 0.0 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth for visibility score calculation", + "default": "360.0", + "minimum": 0.0, + "maximum": 360.0 + }, + "max_distance": { + "type": "number", + "description": "The limit distance for visibility score calculation", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bin for visibility histogram", + "default": "128", + "minimum": 1 + }, + "horizontal_bins": { + "type": "integer", + "description": "The number of horizontal bin for visibility histogram", + "default": "36", + "minimum": 1 + }, + "noise_threshold": { + "type": "integer", + "description": "The threshold value for distinguishing noise from valid points in the frequency image", + "default": "2", + "minimum": 0 + } + }, + "required": [ + "distance_ratio", + "object_length_threshold", + "num_points_threshold", + "max_rings_num", + "max_points_num_per_ring", + "publish_outlier_pointcloud", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "horizontal_bins", + "noise_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ring_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp index a4ccca9beea72..8ae814cf0ef1d 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -49,7 +49,7 @@ * */ -#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp" #include #include @@ -64,9 +64,9 @@ ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent( : Filter("ApproximateDownsampleFilter", options) { { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); } using std::placeholders::_1; diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index 5fd3262088d83..d3f81473ed06f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 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,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -38,29 +38,24 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( { // set initial parameters { - x_max_ = static_cast(declare_parameter("x_max", 18.0)); - x_min_ = static_cast(declare_parameter("x_min", -12.0)); - y_max_ = static_cast(declare_parameter("y_max", 2.0)); - y_min_ = static_cast(declare_parameter("y_min", -2.0)); - z_max_ = static_cast(declare_parameter("z_max", 10.0)); - z_min_ = static_cast(declare_parameter("z_min", 0.0)); - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 135.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 225.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); - weak_first_distance_ratio_ = - static_cast(declare_parameter("weak_first_distance_ratio", 1.004)); - general_distance_ratio_ = - static_cast(declare_parameter("general_distance_ratio", 1.03)); - - weak_first_local_noise_threshold_ = - static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); - roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); - visibility_error_threshold_ = - static_cast(declare_parameter("visibility_error_threshold", 0.5)); - visibility_warn_threshold_ = - static_cast(declare_parameter("visibility_warn_threshold", 0.7)); + x_max_ = declare_parameter("x_max"); + x_min_ = declare_parameter("x_min"); + y_max_ = declare_parameter("y_max"); + y_min_ = declare_parameter("y_min"); + z_max_ = declare_parameter("z_max"); + z_min_ = declare_parameter("z_min"); + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + max_azimuth_diff_ = declare_parameter("max_azimuth_diff"); + weak_first_distance_ratio_ = declare_parameter("weak_first_distance_ratio"); + general_distance_ratio_ = declare_parameter("general_distance_ratio"); + + weak_first_local_noise_threshold_ = declare_parameter("weak_first_local_noise_threshold"); + roi_mode_ = declare_parameter("roi_mode"); + visibility_error_threshold_ = declare_parameter("visibility_error_threshold"); + visibility_warn_threshold_ = declare_parameter("visibility_warn_threshold"); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add( diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index e71158ccf32d6..bf140e662440b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 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,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -47,22 +47,21 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions // set initial parameters { - distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); - 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)); + distance_ratio_ = declare_parameter("distance_ratio"); + object_length_threshold_ = declare_parameter("object_length_threshold"); + num_points_threshold_ = declare_parameter("num_points_threshold"); + max_rings_num_ = static_cast(declare_parameter("max_rings_num")); max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); - publish_outlier_pointcloud_ = - static_cast(declare_parameter("publish_outlier_pointcloud", false)); - - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 0.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 360.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - horizontal_bins_ = static_cast(declare_parameter("horizontal_bins", 36)); - noise_threshold_ = static_cast(declare_parameter("noise_threshold", 2)); + static_cast(declare_parameter("max_points_num_per_ring")); + + publish_outlier_pointcloud_ = declare_parameter("publish_outlier_pointcloud"); + + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + horizontal_bins_ = declare_parameter("horizontal_bins"); + noise_threshold_ = declare_parameter("noise_threshold"); } using std::placeholders::_1; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index 5615f5c118165..b78ec8ccd538a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -15,9 +15,9 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "interpolation/linear_interpolation.hpp" #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index ea0653c879727..381b6ae9da4e2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -15,9 +15,9 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "interpolation/linear_interpolation.hpp" #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" @@ -41,8 +41,8 @@ class AccelerationMap } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); - acc_index_ = CSVLoader::getColumnIndex(table); + vel_index_ = CSVLoader::getColumnIndex(table); + acc_index_ = CSVLoader::getRowIndex(table); acceleration_map_ = CSVLoader::getMap(table); std::cout << "[SimModelDelaySteerMapAccGeared]: success to read acceleration map from " @@ -57,13 +57,14 @@ class AccelerationMap // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : acceleration_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_); - const double acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); + const double acc = autoware::interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); return acc; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp index 1189477a4f45b..b410f089725a2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp @@ -119,7 +119,7 @@ Map CSVLoader::getMap(const Table & table) return map; } -std::vector CSVLoader::getRowIndex(const Table & table) +std::vector CSVLoader::getColumnIndex(const Table & table) { // NOTE: table[0][i] represents the element in the 0-th row and i-th column // This means that we are getting the index of each column in the 0-th row @@ -130,7 +130,7 @@ std::vector CSVLoader::getRowIndex(const Table & table) return index; } -std::vector CSVLoader::getColumnIndex(const Table & table) +std::vector CSVLoader::getRowIndex(const Table & table) { // NOTE: table[i][0] represents the element in the i-th row and 0-th column // This means that we are getting the index of each row in the 0-th column diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 19794c04750fd..74ad765687a2e 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -27,13 +27,10 @@ bool ActuationMap::readActuationMapFromCSV(const std::string & csv_path, const b return false; } - state_index_ = CSVLoader::getRowIndex(table); - actuation_index_ = CSVLoader::getColumnIndex(table); + state_index_ = CSVLoader::getColumnIndex(table); + actuation_index_ = CSVLoader::getRowIndex(table); actuation_map_ = CSVLoader::getMap(table); - if (validation && !CSVLoader::validateMap(actuation_map_, true)) { - return false; - } - return true; + return !validation || CSVLoader::validateMap(actuation_map_, true); } double ActuationMap::getControlCommand(const double actuation, const double state) const @@ -41,13 +38,15 @@ double ActuationMap::getControlCommand(const double actuation, const double stat std::vector interpolated_control_vec{}; const double clamped_state = CSVLoader::clampValue(state, state_index_); - for (std::vector control_command_values : actuation_map_) { + interpolated_control_vec.reserve(actuation_map_.size()); + for (const std::vector & control_command_values : actuation_map_) { interpolated_control_vec.push_back( - interpolation::lerp(state_index_, control_command_values, clamped_state)); + autoware::interpolation::lerp(state_index_, control_command_values, clamped_state)); } const double clamped_actuation = CSVLoader::clampValue(actuation, actuation_index_); - return interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); + return autoware::interpolation::lerp( + actuation_index_, interpolated_control_vec, clamped_actuation); } std::optional AccelMap::getThrottle(const double acc, double vel) const @@ -60,8 +59,10 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const const double clamped_vel = CSVLoader::clampValue(vel, vel_indices); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : accel_map) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + interpolated_acc_vec.reserve(accel_map.size()); + for (const std::vector & accelerations : accel_map) { + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } // calculate throttle @@ -69,11 +70,12 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const // When the desired acceleration is greater than the throttle area, return max throttle if (acc < interpolated_acc_vec.front()) { return std::nullopt; - } else if (interpolated_acc_vec.back() < acc) { + } + if (interpolated_acc_vec.back() < acc) { return throttle_indices.back(); } - return interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); + return autoware::interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); } double BrakeMap::getBrake(const double acc, double vel) const @@ -86,8 +88,10 @@ double BrakeMap::getBrake(const double acc, double vel) const const double clamped_vel = CSVLoader::clampValue(vel, vel_indices); // (brake, vel, acc) map => (brake, acc) map by fixing vel - for (std::vector accelerations : brake_map) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + interpolated_acc_vec.reserve(brake_map.size()); + for (const std::vector & accelerations : brake_map) { + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } // calculate brake @@ -95,12 +99,13 @@ double BrakeMap::getBrake(const double acc, double vel) const // When the desired acceleration is greater than the brake area, return min brake on the map if (acc < interpolated_acc_vec.back()) { return brake_indices.back(); - } else if (interpolated_acc_vec.front() < acc) { + } + if (interpolated_acc_vec.front() < acc) { return brake_indices.front(); } std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); - return interpolation::lerp(interpolated_acc_vec, brake_indices, acc); + return autoware::interpolation::lerp(interpolated_acc_vec, brake_indices, acc); } // steer map sim model diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index aada6687baef8..88b0e6c639fd1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -69,7 +69,6 @@ double SimModelDelaySteerAccGeared::getAx() double SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerAccGeared::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp index 6b993b8b24409..78cfa6caeb946 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp @@ -69,7 +69,6 @@ double SimModelDelaySteerAccGearedWoFallGuard::getAx() double SimModelDelaySteerAccGearedWoFallGuard::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerAccGearedWoFallGuard::getSteer() { diff --git a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json similarity index 93% rename from system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json rename to system/bluetooth_monitor/schema/bluetooth_monitor.schema.json index 6951ecd6aed88..0914c7ec9a9b9 100644 --- a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json +++ b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json @@ -6,7 +6,7 @@ "bluetooth_monitor": { "type": "object", "properties": { - "address": { + "addresses": { "type": "array", "description": "Bluetooth addresses of the device to monitor", "items": { @@ -30,7 +30,7 @@ "default": 0.1 } }, - "required": ["address", "port", "timeout", "rtt_warn"], + "required": ["addresses", "port", "timeout", "rtt_warn"], "additionalProperties": false } }, 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 c04503bee70fd..74c5e6f84e80b 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -29,3 +29,7 @@ The clear API is called automatically before setting the route. | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | | Client | - | /api/routing/change_route_points | The route points change API. | + +## parameters + +{{ json_to_markdown("/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json") }} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json b/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json new file mode 100644 index 0000000000000..bdc921e31dd23 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json @@ -0,0 +1,37 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "ad_api_adaptors parameter", + "type": "object", + "definitions": { + "ad_api_adaptors": { + "type": "object", + "properties": { + "initial_pose_particle_covariance": { + "type": "array", + "description": "initial_pose_particle_covariance", + "default": [ + 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0 + ] + } + }, + "required": ["initial_pose_particle_covariance"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/initial_pose_particle_covariance" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 4ed4fbee7ac81..86a76c397c728 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -22,9 +22,9 @@ autoware_cmake autoware_control_msgs + autoware_interpolation autoware_vehicle_msgs geometry_msgs - interpolation nav_msgs rclcpp rclcpp_components diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp index 236aa7dc451c3..bc012932a9b0d 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp @@ -14,15 +14,11 @@ #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" -#include -#include #include #include -using namespace std::literals::chrono_literals; - namespace autoware::raw_vehicle_cmd_converter { bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool validation) @@ -35,13 +31,10 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool vali } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); - throttle_index_ = CSVLoader::getColumnIndex(table); + vel_index_ = CSVLoader::getColumnIndex(table); + throttle_index_ = CSVLoader::getRowIndex(table); accel_map_ = CSVLoader::getMap(table); - if (validation && !CSVLoader::validateMap(accel_map_, true)) { - return false; - } - return true; + return !validation || CSVLoader::validateMap(accel_map_, true); } bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const @@ -49,19 +42,22 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons std::vector interpolated_acc_vec; const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.reserve(accel_map_.size()); + for (const std::vector & accelerations : accel_map_) { + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return false => brake sequence // When the desired acceleration is greater than the throttle area, return max throttle if (acc < interpolated_acc_vec.front()) { return false; - } else if (interpolated_acc_vec.back() < acc) { + } + if (interpolated_acc_vec.back() < acc) { throttle = throttle_index_.back(); return true; } - throttle = interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); + throttle = autoware::interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); return true; } @@ -71,15 +67,16 @@ bool AccelMap::getAcceleration(const double throttle, const double vel, double & const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + interpolated_acc_vec.reserve(accel_map_.size()); for (const auto & acc_vec : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); - acc = interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); + acc = autoware::interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); return true; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp index ae0f8e5f41b1c..343804d4a0ff5 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -32,8 +32,8 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path, const bool vali } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); - brake_index_ = CSVLoader::getColumnIndex(table); + vel_index_ = CSVLoader::getColumnIndex(table); + brake_index_ = CSVLoader::getRowIndex(table); brake_map_ = CSVLoader::getMap(table); brake_index_rev_ = brake_index_; if (validation && !CSVLoader::validateMap(brake_map_, false)) { @@ -50,8 +50,10 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : brake_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.reserve(brake_map_.size()); + for (const std::vector & accelerations : brake_map_) { + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate brake @@ -65,13 +67,14 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) acc, interpolated_acc_vec.back()); brake = brake_index_.back(); return true; - } else if (interpolated_acc_vec.front() < acc) { + } + if (interpolated_acc_vec.front() < acc) { brake = brake_index_.front(); return true; } std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); - brake = interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); + brake = autoware::interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); return true; } @@ -82,15 +85,16 @@ bool BrakeMap::getAcceleration(const double brake, const double vel, double & ac const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + interpolated_acc_vec.reserve(brake_map_.size()); for (const auto & acc_vec : brake_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate brake // When the desired acceleration is smaller than the brake area, return min acc // When the desired acceleration is greater than the brake area, return min acc const double clamped_brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); - acc = interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); + acc = autoware::interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); return true; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp index 8044a88bc9898..0a31e2089e4ef 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -15,7 +15,6 @@ #include "autoware_raw_vehicle_cmd_converter/csv_loader.hpp" #include -#include #include #include @@ -48,10 +47,7 @@ bool CSVLoader::readCSV(Table & result, const char delim) result.push_back(tokens); } } - if (!validateData(result, csv_path_)) { - return false; - } - return true; + return validateData(result, csv_path_); } bool CSVLoader::validateMap(const Map & map, const bool is_col_decent) @@ -119,7 +115,7 @@ Map CSVLoader::getMap(const Table & table) return map; } -std::vector CSVLoader::getRowIndex(const Table & table) +std::vector CSVLoader::getColumnIndex(const Table & table) { std::vector index = {}; for (size_t i = 1; i < table[0].size(); i++) { @@ -128,7 +124,7 @@ std::vector CSVLoader::getRowIndex(const Table & table) return index; } -std::vector CSVLoader::getColumnIndex(const Table & table) +std::vector CSVLoader::getRowIndex(const Table & table) { std::vector index = {}; for (size_t i = 1; i < table.size(); i++) { diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp index ebc608cd61fad..6b2bffa7630f9 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp @@ -14,6 +14,8 @@ #include "autoware_raw_vehicle_cmd_converter/pid.hpp" +#include + #include #include @@ -25,7 +27,7 @@ double PIDController::calculateFB( const double current_value, std::vector & pid_contributions, std::vector & errors) { const double error = target_value - current_value; - const bool enable_integration = (std::abs(reset_trigger_value) < 0.01) ? false : true; + const bool enable_integration = std::abs(reset_trigger_value) >= 0.01; return calculatePID(error, dt, enable_integration, pid_contributions, errors, false); } @@ -48,7 +50,7 @@ double PIDController::calculatePID( } double ret_i = ki_ * error_integral_; - double error_differential; + double error_differential = 0.0; if (is_first_time_) { error_differential = 0; is_first_time_ = false; diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp index 6abe8adfdc9e3..9777b4f24a1fa 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -32,25 +32,24 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool vali } vehicle_name_ = table[0][0]; - steer_index_ = CSVLoader::getRowIndex(table); - output_index_ = CSVLoader::getColumnIndex(table); + steer_index_ = CSVLoader::getColumnIndex(table); + output_index_ = CSVLoader::getRowIndex(table); steer_map_ = CSVLoader::getMap(table); - if (validation && !CSVLoader::validateMap(steer_map_, true)) { - return false; - } - return true; + return !validation || CSVLoader::validateMap(steer_map_, true); } void SteerMap::getSteer(const double steer_rate, const double steer, double & output) const { const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); std::vector steer_rate_interp = {}; + steer_rate_interp.reserve(steer_map_.size()); for (const auto & steer_rate_vec : steer_map_) { - steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); + steer_rate_interp.push_back( + autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); } const double clamped_steer_rate = CSVLoader::clampValue(steer_rate, steer_rate_interp, "steer: steer_rate"); - output = interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); + output = autoware::interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); } } // namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv b/vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp index 59ab3a880fabd..0d716eef3369b 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp @@ -21,7 +21,6 @@ #include "gtest/gtest.h" #include -#include /* * Throttle data: (vel, throttle -> acc) @@ -124,6 +123,9 @@ TEST(ConverterTests, LoadValidPath) EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv", true)); EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv", true)); EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv", true)); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_empty_map.csv", true)); + + EXPECT_FALSE(steer_map.readSteerMapFromCSV(map_path + "test_not_interpolatable.csv", true)); } TEST(ConverterTests, AccelMapCalculation) @@ -136,6 +138,16 @@ TEST(ConverterTests, AccelMapCalculation) return output; }; + // for get function in acceleration + std::vector map_column_idx = {0.0, 5.0, 10.0}; + std::vector map_raw_idx = {0.0, 0.5, 1.0}; + std::vector> map_value = { + {0.0, -0.3, -0.5}, {1.0, 0.5, 0.0}, {3.0, 2.0, 1.5}}; + + EXPECT_EQ(accel_map.getVelIdx(), map_column_idx); + EXPECT_EQ(accel_map.getThrottleIdx(), map_raw_idx); + EXPECT_EQ(accel_map.getAccelMap(), map_value); + // case for max vel nominal acc EXPECT_DOUBLE_EQ(calcThrottle(0.0, 20.0), 0.5); @@ -148,6 +160,9 @@ TEST(ConverterTests, AccelMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcThrottle(2.0, 0.0), 0.75); + // case for max throttle + EXPECT_DOUBLE_EQ(calcThrottle(2.0, 10.0), 1.0); + const auto calcAcceleration = [&](double throttle, double vel) { double output; accel_map.getAcceleration(throttle, vel, output); @@ -177,6 +192,15 @@ TEST(ConverterTests, BrakeMapCalculation) return output; }; + // for get function in brake + std::vector map_column_idx = {0.0, 5.0, 10.0}; + std::vector map_raw_idx = {0.0, 0.5, 1.0}; + std::vector> map_value = { + {0.0, -0.4, -0.5}, {-1.5, -2.0, -2.0}, {-2.0, -2.5, -3.0}}; + EXPECT_EQ(brake_map.getVelIdx(), map_column_idx); + EXPECT_EQ(brake_map.getBrakeIdx(), map_raw_idx); + EXPECT_EQ(brake_map.getBrakeMap(), map_value); + // case for min vel min acc EXPECT_DOUBLE_EQ(calcBrake(-2.5, 0.0), 1.0); @@ -189,6 +213,9 @@ TEST(ConverterTests, BrakeMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcBrake(-2.25, 5.0), 0.75); + // case for min brake + EXPECT_DOUBLE_EQ(calcBrake(1.0, 5.0), 0.0); + const auto calcAcceleration = [&](double brake, double vel) { double output; brake_map.getAcceleration(brake, vel, output);