diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 01ccc632b0133..3045b3746ea8e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -154,8 +154,13 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp -planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_goal_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner_common/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index f9c29b81b6e6c..1da4d24966de9 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.11.0 + uses: styfle/cancel-workflow-action@0.12.0 with: workflow_id: all all_but_latest: true diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 95ebb8725f62b..b426d0cba6614 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -30,7 +30,7 @@ jobs: echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 ref: ${{ steps.set-tag-name.outputs.ref-name }} diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index e754ecab24f85..38738196a0bd3 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index b231dbda87938..33c00ee1066ae 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -16,7 +16,7 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ github.event.pull_request.head.ref }} diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index eb18ccdba38d0..1fbf2ff46925c 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Run spell-check uses: autowarefoundation/autoware-github-actions/spell-check@v1 diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 95bbcd93e30df..be5a70ed7ffc9 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -20,9 +20,11 @@ #include #include +#include #include #include #include + namespace motion_utils { @@ -38,7 +40,7 @@ struct VirtualWall double longitudinal_offset{}; bool is_driving_forward{true}; }; -typedef std::vector VirtualWalls; +using VirtualWalls = std::vector; /// @brief class to manage the creation of virtual wall markers /// @details creates both ADD and DELETE markers @@ -55,8 +57,8 @@ class VirtualWallMarkerCreator const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward)>; - VirtualWalls virtual_walls; - std::unordered_map marker_count_per_namespace; + VirtualWalls virtual_walls_; + std::unordered_map marker_count_per_namespace_; /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map void cleanup(); diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 106ed32b18576..258386ca236a3 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,13 +15,10 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include -#include -#include #include namespace motion_utils @@ -187,7 +184,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, - const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); + const bool use_zero_order_hold_for_twist = true, + const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a trajectory. Note that in a default setting, position xy are diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 1e4d0d633325e..87a0d54af6619 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -23,26 +23,19 @@ namespace resample_utils { -constexpr double CLOSE_S_THRESHOLD = 1e-6; +constexpr double close_s_threshold = 1e-6; template bool validate_size(const T & points) { - if (points.size() < 2) { - return false; - } - return true; + return points.size() >= 2; } template bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) { const double points_length = motion_utils::calcArcLength(points); - if (points_length < resampling_intervals.back()) { - return false; - } - - return true; + return points_length >= resampling_intervals.back(); } template @@ -52,7 +45,7 @@ bool validate_points_duplication(const T & points) const auto & curr_pt = tier4_autoware_utils::getPoint(points.at(i)); const auto & next_pt = tier4_autoware_utils::getPoint(points.at(i + 1)); const double ds = tier4_autoware_utils::calcDistance2d(curr_pt, next_pt); - if (ds < CLOSE_S_THRESHOLD) { + if (ds < close_s_threshold) { return false; } } @@ -67,7 +60,8 @@ bool validate_arguments(const T & input_points, const std::vector & resa if (!validate_size(input_points)) { std::cerr << "The number of input points is less than 2" << std::endl; return false; - } else if (!validate_size(resampling_intervals)) { + } + if (!validate_size(resampling_intervals)) { std::cerr << "The number of resampling intervals is less than 2" << std::endl; return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index c23478605c41a..aec329f9f540e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -16,16 +16,12 @@ #define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include #include -#include -#include -#include namespace motion_utils { diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index e088285147735..d0ed761b76d9a 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include #include diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp index 6921c56e0199e..28d783aa4d293 100644 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp @@ -15,8 +15,8 @@ #ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index cb815f7dd0a0c..f2e0c1c0c184c 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -130,11 +130,11 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) if (points_with_twist.size() == 1) { if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { return true; - } else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + } + if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; - } else { - return std::nullopt; } + return std::nullopt; } return isDrivingForward(points_with_twist); @@ -401,12 +401,12 @@ double calcLongitudinalOffsetToSegment( const bool throw_exception = false) { if (seg_idx >= points.size() - 1) { - const std::out_of_range e("Segment index is invalid."); + const auto error_message{"Segment index is invalid."}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return std::nan(""); } @@ -424,12 +424,12 @@ double calcLongitudinalOffsetToSegment( } if (seg_idx >= overlap_removed_points.size() - 1) { - const std::runtime_error e("Same points are given."); + const auto error_message{"Same points are given."}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return std::nan(""); } @@ -581,12 +581,12 @@ double calcLateralOffset( } if (overlap_removed_points.size() == 1) { - const std::runtime_error e("Same points are given."); + const auto error_message{"Same points are given."}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return std::nan(""); } @@ -643,12 +643,12 @@ double calcLateralOffset( } if (overlap_removed_points.size() == 1) { - const std::runtime_error e("Same points are given."); + const auto error_message{"Same points are given."}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return std::nan(""); } @@ -968,7 +968,7 @@ std::vector> calcCurvatureAndArcLength(const T & point { // Note that arclength is for the segment, not the sum. std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + curvature_arc_length_vec.emplace_back(0.0, 0.0); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); @@ -976,9 +976,9 @@ std::vector> calcCurvatureAndArcLength(const T & point const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.push_back(std::pair(curvature, arc_length)); + curvature_arc_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + curvature_arc_length_vec.emplace_back(0.0, 0.0); return curvature_arc_length_vec; } @@ -1046,12 +1046,12 @@ std::optional calcLongitudinalOffsetPoint( } if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); + const auto error_message{"Invalid source index"}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return {}; } @@ -1171,12 +1171,12 @@ std::optional calcLongitudinalOffsetPose( } if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); + const auto error_message{"Invalid source index"}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return {}; } @@ -2041,9 +2041,8 @@ size_t findFirstNearestIndexWithSoftConstraints( if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { break; - } else { - continue; } + continue; } if (min_squared_dist <= squared_dist) { @@ -2073,9 +2072,8 @@ size_t findFirstNearestIndexWithSoftConstraints( if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { break; - } else { - continue; } + continue; } if (min_squared_dist <= squared_dist) { @@ -2386,12 +2384,12 @@ double calcYawDeviation( } if (overlap_removed_points.size() <= 1) { - const std::runtime_error e("points size is less than 2"); + const auto error_message{"points size is less than 2"}; tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + std::cerr << error_message << std::endl; return 0.0; } diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 6a9bd01a5a9a0..82433d8e3c241 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -22,7 +22,6 @@ #include #include -#include namespace motion_utils { diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index c6de95dd568cf..3ac37bbc7733b 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/motion_utils/src/distance/distance.cpp @@ -262,7 +262,8 @@ std::optional calcDecelDistWithJerkAndAccConstraints( if (t_during_min_acc > epsilon) { return calcDecelDistPlanType1( current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); - } else if (is_decel_needed || current_acc > epsilon) { + } + if (is_decel_needed || current_acc > epsilon) { return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); } diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 50def253b6d16..6e9c45adc241b 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,14 +14,12 @@ #include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample_utils.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include +#include -#include +#include -using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createDeletedDefaultMarker; using tier4_autoware_utils::createMarkerColor; diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index c22aaa0d5e87c..34aae096ffa9e 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -21,24 +21,24 @@ namespace motion_utils void VirtualWallMarkerCreator::cleanup() { - for (auto it = marker_count_per_namespace.begin(); it != marker_count_per_namespace.end();) { + for (auto it = marker_count_per_namespace_.begin(); it != marker_count_per_namespace_.end();) { const auto & marker_count = it->second; const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0; if (is_unused_namespace) - it = marker_count_per_namespace.erase(it); + it = marker_count_per_namespace_.erase(it); else ++it; } - virtual_walls.clear(); + virtual_walls_.clear(); } void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall) { - virtual_walls.push_back(virtual_wall); + virtual_walls_.push_back(virtual_wall); } void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls) { - virtual_walls.insert(virtual_walls.end(), walls.begin(), walls.end()); + virtual_walls_.insert(virtual_walls_.end(), walls.begin(), walls.end()); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( @@ -46,13 +46,13 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( { visualization_msgs::msg::MarkerArray marker_array; // update marker counts - for (auto & [ns, count] : marker_count_per_namespace) { + for (auto & [ns, count] : marker_count_per_namespace_) { count.previous = count.current; count.current = 0UL; } // convert to markers create_wall_function create_fn; - for (const auto & virtual_wall : virtual_walls) { + for (const auto & virtual_wall : virtual_walls_) { switch (virtual_wall.style) { case stop: create_fn = motion_utils::createStopVirtualWallMarker; @@ -68,15 +68,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { - marker.id = marker_count_per_namespace[marker.ns].current++; + marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); } } // create delete markers visualization_msgs::msg::Marker marker; marker.action = visualization_msgs::msg::Marker::DELETE; - for (const auto & [ns, count] : marker_count_per_namespace) { - for (marker.id = count.current; marker.id < static_cast(count.previous); ++marker.id) { + for (const auto & [ns, count] : marker_count_per_namespace_) { + for (marker.id = static_cast(count.current); marker.id < static_cast(count.previous); + ++marker.id) { marker.ns = ns; marker_array.markers.push_back(marker); } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 6bada921804d4..1566b493c82f8 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,15 +14,12 @@ #include "motion_utils/resample/resample.hpp" -#include "interpolation/interpolation_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" #include "motion_utils/resample/resample_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" namespace motion_utils { diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 4077202aba8a4..6ee3e665f746a 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -15,8 +15,6 @@ #include "motion_utils/trajectory/interpolation.hpp" #include "interpolation/linear_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" using autoware_auto_planning_msgs::msg::PathPointWithLaneId; @@ -34,7 +32,8 @@ TrajectoryPoint calcInterpolatedPoint( TrajectoryPoint interpolated_point{}; interpolated_point.pose = target_pose; return interpolated_point; - } else if (trajectory.points.size() == 1) { + } + if (trajectory.points.size() == 1) { return trajectory.points.front(); } @@ -101,7 +100,8 @@ PathPointWithLaneId calcInterpolatedPoint( PathPointWithLaneId interpolated_point{}; interpolated_point.point.pose = target_pose; return interpolated_point; - } else if (path.points.size() == 1) { + } + if (path.points.size() == 1) { return path.points.front(); } diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/tmp_conversion.cpp index 5c907a5926046..6f4a4039832bd 100644 --- a/common/motion_utils/src/trajectory/tmp_conversion.cpp +++ b/common/motion_utils/src/trajectory/tmp_conversion.cpp @@ -14,9 +14,6 @@ #include "motion_utils/trajectory/tmp_conversion.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" - #include namespace motion_utils @@ -36,12 +33,7 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( const std::vector & trajectory) { autoware_auto_planning_msgs::msg::Trajectory output{}; - for (const auto & pt : trajectory) { - output.points.push_back(pt); - if (output.points.size() >= output.CAPACITY) { - break; - } - } + for (const auto & pt : trajectory) output.points.push_back(pt); return output; } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 1786586ec39a8..22567b569d0ad 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -803,17 +803,6 @@ TEST(trajectory, convertToTrajectory) const auto traj = convertToTrajectory(traj_input); EXPECT_EQ(traj.points.size(), traj_input.size()); } - - // Clipping check - { - const auto traj_input = generateTestTrajectoryPointArray(10000, 1.0); - const auto traj = convertToTrajectory(traj_input); - EXPECT_EQ(traj.points.size(), traj.CAPACITY); - // Value check - for (size_t i = 0; i < traj.points.size(); ++i) { - EXPECT_EQ(traj.points.at(i), traj_input.at(i)); - } - } } TEST(trajectory, convertToTrajectoryPointArray) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index 11dd050658aeb..17955fc5d0b9b 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -38,9 +38,13 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance - behavior_path_planner_lane_change: + behavior_path_avoidance_by_lane_change: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change + logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE + + behavior_path_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.NORMAL behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 12f0eece1e477..698d4a8403069 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -646,10 +646,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in EMERGENCY state if (m_control_state == ControlState::EMERGENCY) { + if (stopped_condition) { + return changeState(ControlState::STOPPED); + } + if (!emergency_condition) { - if (stopped_condition) { - return changeState(ControlState::STOPPED); - } if (!is_under_control) { // NOTE: On manual driving, no need stopping to exit the emergency. return changeState(ControlState::DRIVE); diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 8538179253480..3fba4bd773e68 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 181f470a00800..ac7589ea2273b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -1,17 +1,13 @@ - - - - - - - - - - - - + + + + + + + + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index d8f69c124526a..f75a181bfb659 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -7,6 +7,7 @@ Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda + Kento Yabuuchi Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 0d03e3c8cdcd9..0649d8c5d5116 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -104,6 +104,7 @@ + @@ -132,6 +133,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 07b53fb671732..85c6e87c0fd19 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -1,8 +1,6 @@ - - @@ -19,8 +17,7 @@ - - + @@ -29,15 +26,12 @@ - - + - - diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 12190fb659235..fecdd29bcb7e9 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -78,7 +78,7 @@ def create_additional_pipeline(self, lidar_name): plugin="pointcloud_preprocessor::CropBoxFilterComponent", name=f"{lidar_name}_crop_box_filter", remappings=[ - ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), + ("input", f"/sensing/lidar/{lidar_name}/pointcloud"), ("output", f"{lidar_name}/range_cropped/pointcloud"), ], parameters=[ @@ -474,7 +474,13 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, def launch_setup(context, *args, **kwargs): pipeline = GroundSegmentationPipeline(context) - components = [] + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + + components = [glog_component] components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 96ad80131e4e7..7f5f9d61c4bf0 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -14,6 +14,7 @@ + @@ -177,6 +178,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3a1f417106f54..b36b40ed6a980 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -43,6 +43,8 @@ + + - + @@ -174,7 +176,7 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> - + @@ -208,7 +210,6 @@ - diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e2ffff2195645..ba18b7dadd599 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -155,7 +155,6 @@ class EKFLocalizer : public rclcpp::Node const HyperParameters params_; - double ekf_rate_; double ekf_dt_; /* process noise variance for discrete model */ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 873060c75fcfc..e88a59ffdfab9 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -30,6 +30,7 @@ #include #include +#include struct EKFDiagnosticInfo { @@ -76,12 +77,15 @@ class EKFModule std::array getCurrentPoseCovariance() const; std::array getCurrentTwistCovariance() const; + size_t find_closest_delay_time_index(double target_value) const; + void accumulate_delay_time(const double dt); + void predictWithDelay(const double dt); bool measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); bool measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( const PoseWithCovariance & pose, const double delay_time); @@ -91,6 +95,7 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; + std::vector accumulated_delay_times_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index f6c664eb26451..e1eafdc6f7948 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,10 +17,9 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); + const double delay_time, const double delay_time_threshold); std::string poseDelayTimeWarningMessage(const double delay_time); std::string twistDelayTimeWarningMessage(const double delay_time); std::string mahalanobisWarningMessage(const double distance, const double max_distance); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5dd78a00f2fbd..a3d2f52a4058c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti : rclcpp::Node(node_name, node_options), warning_(std::make_shared(this)), params_(this), - ekf_rate_(params_.ekf_rate), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps) @@ -109,9 +108,22 @@ void EKFLocalizer::updatePredictFrequency() if (get_clock()->now() < *last_predict_time_) { warning_->warn("Detected jump back in time"); } else { - ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); - DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); - ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + /* Measure dt */ + ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds(); + DEBUG_INFO( + get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); + + if (ekf_dt_ > 10.0) { + ekf_dt_ = 10.0; + RCLCPP_WARN( + get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); + } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + RCLCPP_WARN( + get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); + } + + /* Register dt and accumulate time delay */ + ekf_module_->accumulate_delay_time(ekf_dt_); /* Update discrete proc_cov*/ proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); @@ -165,7 +177,7 @@ void EKFLocalizer::timerCallback() const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); if (is_updated) { pose_is_updated = true; @@ -200,8 +212,7 @@ void EKFLocalizer::timerCallback() const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = - ekf_module_->measurementUpdateTwist(*twist, ekf_dt_, t_curr, twist_diag_info_); + bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); if (is_updated) { twist_is_updated = true; } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 0a5e3c98c96c8..573472fe2dabf 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -38,6 +38,7 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz + accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -131,6 +132,48 @@ double EKFModule::getYawBias() const return kalman_filter_.getLatestX()(IDX::YAWB); } +size_t EKFModule::find_closest_delay_time_index(double target_value) const +{ + // If target_value is too large, return last index + 1 + if (target_value > accumulated_delay_times_.back()) { + return accumulated_delay_times_.size(); + } + + auto lower = std::lower_bound( + accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value); + + // If the lower bound is the first element, return its index. + // If the lower bound is beyond the last element, return the last index. + // If else, take the closest element. + if (lower == accumulated_delay_times_.begin()) { + return 0; + } else if (lower == accumulated_delay_times_.end()) { + return accumulated_delay_times_.size() - 1; + } else { + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); + } +} + +void EKFModule::accumulate_delay_time(const double dt) +{ + // Shift the delay times to the right. + std::copy_backward( + accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1, + accumulated_delay_times_.end()); + + // Add a new element (=0) and, and add delay time to the previous elements. + accumulated_delay_times_.front() = 0.0; + for (size_t i = 1; i < accumulated_delay_times_.size(); ++i) { + accumulated_delay_times_[i] += dt; + } +} + void EKFModule::predictWithDelay(const double dt) { const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); @@ -147,8 +190,7 @@ void EKFModule::predictWithDelay(const double dt) } bool EKFModule::measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & pose_diag_info) + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { warning_->warnThrottle( @@ -170,14 +212,15 @@ bool EKFModule::measurementUpdatePose( delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); - pose_diag_info.delay_time_threshold = params_.extend_state_step * dt; + pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; warning_->warnThrottle( - poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + 2000); return false; } @@ -243,7 +286,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela } bool EKFModule::measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { @@ -262,14 +305,16 @@ bool EKFModule::measurementUpdateTwist( } delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); - twist_diag_info.delay_time_threshold = params_.extend_state_step * dt; + twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; warning_->warnThrottle( - twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + twistDelayStepWarningMessage( + twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), + 2000); return false; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 9a7bbf47a94eb..c69f30b2d6601 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,22 +18,20 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } std::string poseDelayTimeWarningMessage(const double delay_time) diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index b4a05afa844dc..2069969e0ae5e 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -21,17 +21,17 @@ TEST(PoseDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(), + poseDelayStepWarningMessage(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " - "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]"); + "delay: 6.000[s], limit: 4.000[s]"); } TEST(TwistDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(), + twistDelayStepWarningMessage(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " - "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]"); + "delay: 10.000[s], limit: 6.000[s]"); } TEST(PoseDelayTimeWarningMessage, SmokeTest) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 6437455875cc2..272338905c3f0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -8,18 +8,24 @@ - + + + + - + + + + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 7e83220dadf2a..857ec4f16f051 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -17,7 +17,6 @@ aruco cv_bridge diagnostic_msgs - image_transport landmark_manager lanelet2_extension localization_util diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index e569a71bbb5b0..a1e2f3ec31dd3 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -66,10 +66,6 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : Node("ar_tag_based_localizer", options), cam_info_received_(false) -{ -} - -bool ArTagBasedLocalizer::setup() { /* Declare node parameters @@ -92,7 +88,7 @@ bool ArTagBasedLocalizer::setup() } else { // Error RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); - return false; + return; } ekf_pose_buffer_ = std::make_unique( this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); @@ -111,59 +107,50 @@ bool ArTagBasedLocalizer::setup() tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); - /* - Initialize image transport - */ - it_ = std::make_unique(shared_from_this()); - /* Subscribers */ + using std::placeholders::_1; map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); + std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); - pose_array_pub_ = this->create_publisher("~/debug/detected_landmarks", qos_sub); image_sub_ = this->create_subscription( - "~/input/image", qos_sub, - std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); + "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, _1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", qos_sub, - std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, _1)); ekf_pose_sub_ = this->create_subscription( - "~/input/ekf_pose", qos_sub, - std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); + "~/input/ekf_pose", qos_sub, std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, _1)); /* Publishers */ - rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); - qos_marker.transient_local(); - qos_marker.reliable(); - marker_pub_ = this->create_publisher("~/debug/marker", qos_marker); - rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - image_pub_ = it_->advertise("~/debug/result", 1); - pose_pub_ = - this->create_publisher("~/output/pose_with_covariance", qos_pub); - diag_pub_ = this->create_publisher("/diagnostics", qos_pub); + const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); + const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + pose_pub_ = this->create_publisher( + "~/output/pose_with_covariance", qos_pub_periodic); + image_pub_ = this->create_publisher("~/debug/image", qos_pub_periodic); + mapped_tag_pose_pub_ = this->create_publisher("~/debug/mapped_tag", qos_pub_once); + detected_tag_pose_pub_ = + this->create_publisher("~/debug/detected_tag", qos_pub_periodic); + diag_pub_ = this->create_publisher("/diagnostics", qos_pub_periodic); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); - return true; } void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger()); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); - marker_pub_->publish(marker_msg); + mapped_tag_pose_pub_->publish(marker_msg); } void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { // check subscribers - if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { + if ((image_pub_->get_subscription_count() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); return; } @@ -192,7 +179,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) } // for debug - if (pose_array_pub_->get_subscription_count() > 0) { + if (detected_tag_pose_pub_->get_subscription_count() > 0) { PoseArray pose_array_msg; pose_array_msg.header.stamp = sensor_stamp; pose_array_msg.header.frame_id = "map"; @@ -201,7 +188,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) tier4_autoware_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } - pose_array_pub_->publish(pose_array_msg); + detected_tag_pose_pub_->publish(pose_array_msg); } // calc new_self_pose @@ -267,24 +254,12 @@ void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & m return; } - cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - camera_matrix.at(0, 0) = msg->p[0]; - camera_matrix.at(0, 1) = msg->p[1]; - camera_matrix.at(0, 2) = msg->p[2]; - camera_matrix.at(0, 3) = msg->p[3]; - camera_matrix.at(1, 0) = msg->p[4]; - camera_matrix.at(1, 1) = msg->p[5]; - camera_matrix.at(1, 2) = msg->p[6]; - camera_matrix.at(1, 3) = msg->p[7]; - camera_matrix.at(2, 0) = msg->p[8]; - camera_matrix.at(2, 1) = msg->p[9]; - camera_matrix.at(2, 2) = msg->p[10]; - camera_matrix.at(2, 3) = msg->p[11]; - - cv::Mat distortion_coeff(4, 1, CV_64FC1); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } + // copy camera matrix + cv::Mat camera_matrix(3, 4, CV_64FC1); + std::copy(msg->p.begin(), msg->p.end(), camera_matrix.begin()); + + // all 0 + cv::Mat distortion_coeff(4, 1, CV_64FC1, 0.0); const cv::Size size(static_cast(msg->width), static_cast(msg->height)); @@ -361,12 +336,12 @@ std::vector ArTagBasedLocalizer::detect_landmarks( } // for debug - if (image_pub_.getNumSubscribers() > 0) { + if (image_pub_->get_subscription_count() > 0) { cv_bridge::CvImage out_msg; out_msg.header.stamp = sensor_stamp; out_msg.encoding = sensor_msgs::image_encodings::RGB8; out_msg.image = in_image; - image_pub_.publish(out_msg.toImageMsg()); + image_pub_->publish(*out_msg.toImageMsg()); } return landmarks; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index b7dfb45a26ce3..eb7406f8c77f8 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -48,11 +48,12 @@ #include "landmark_manager/landmark_manager.hpp" #include "localization_util/smart_pose_buffer.hpp" -#include #include #include #include +#include +#include #include #include @@ -82,7 +83,6 @@ class ArTagBasedLocalizer : public rclcpp::Node public: explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - bool setup(); private: void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); @@ -104,9 +104,6 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - // image transport - std::unique_ptr it_; - // Subscribers rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; @@ -114,10 +111,10 @@ class ArTagBasedLocalizer : public rclcpp::Node rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers - rclcpp::Publisher::SharedPtr marker_pub_; - rclcpp::Publisher::SharedPtr pose_array_pub_; - image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr detected_tag_pose_pub_; + rclcpp::Publisher::SharedPtr mapped_tag_pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; // Others diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp index cbcd57b4b222a..8ef1dd6195580 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp @@ -48,7 +48,6 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); std::shared_ptr ptr = std::make_shared(); - ptr->setup(); rclcpp::spin(ptr); rclcpp::shutdown(); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 1b44327fd9e8b..5d05dd7e3755a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -55,7 +55,8 @@ class TestArTagBasedLocalizer : public ::testing::Test TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT { - EXPECT_TRUE(node_->setup()); + // Check if the constructor finishes successfully + EXPECT_TRUE(true); } int main(int argc, char ** argv) diff --git a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt index 1b6126c892d2e..1b57e5cc89018 100644 --- a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt @@ -12,7 +12,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(landmark_manager +ament_auto_add_library(landmark_manager SHARED src/landmark_manager.cpp ) diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp index ba293109dcc4d..bc62bfa690946 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -39,10 +39,16 @@ std::optional SmartPoseBuffer::interpolate( const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - if (target_ros_time < time_first || time_last < target_ros_time) { + if (target_ros_time < time_first) { return std::nullopt; } + // [time_last < target_ros_time] is acceptable here. + // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest + // timestamp of buffered pose (often EKF). + // However, if the timestamp difference is too large, + // it will later be rejected by validate_time_stamp_difference. + // get the nearest poses result.old_pose = *pose_buffer_.front(); for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index f50eba9218d67..2dc864b0cb925 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -7,6 +7,7 @@ Takagi, Isamu Yamato Ando Masahiro Sakamoto + Kento Yabuuchi Apache License 2.0 Takagi, Isamu diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 5230d4bd03214..c8fcce6f7002f 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -8,6 +8,7 @@ Ryu Yamamoto Koji Minoda Masahiro Sakamoto + Kento Yabuuchi Apache License 2.0 diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 082dc95d6500a..e14defcb47f26 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -224,6 +224,9 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b128c2cf15e15..76e40d4379de4 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -7,6 +7,7 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi Apache License 2.0 ament_cmake_auto diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 6dc68911699aa..6e266b0ad82f0 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -4,7 +4,9 @@ map_tf_generator 0.1.0 map_tf_generator package as a ROS 2 node - azumi-suzuki + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_auto diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 4655cf4c2f73a..d56cb3fb31584 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -89,6 +89,8 @@ install( RUNTIME DESTINATION bin ) -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch + config ) diff --git a/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml new file mode 100644 index 0000000000000..3d5270d17479e --- /dev/null +++ b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + map_layer_name: "elevation" + height_diff_thresh: 0.15 + map_frame: "map" diff --git a/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..f95c0b0c94b92 --- /dev/null +++ b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml index 42478c514b788..9cb41c8780f13 100644 --- a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml index 4a272646503f1..cdf17ebfeda9f 100644 --- a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml index 2942781ce8123..e3f4594729c2b 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index 2fc2884fd5df9..01ad5cd9b1792 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -1,25 +1,16 @@ + + - - - - - - - - - - - - + diff --git a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml index 09ea9022b97e5..13023a8c1c2c9 100644 --- a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json new file mode 100644 index 0000000000000..24ac2481a4e4a --- /dev/null +++ b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Compare Elevation Map Filter", + "type": "object", + "definitions": { + "compare_elevation_map_filter": { + "type": "object", + "properties": { + "map_layer_name": { + "type": "string", + "default": "elevation", + "description": "elevation map layer name" + }, + "height_diff_thresh": { + "type": "number", + "default": "0.15", + "description": "Remove points whose height difference is below this value [m]" + }, + "map_frame": { + "type": "string", + "default": "map", + "description": "frame_id of the map that is temporarily used before elevation_map is subscribed" + } + }, + "required": ["map_layer_name", "height_diff_thresh", "map_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/compare_elevation_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..10454b48d3d80 --- /dev/null +++ b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Distance Based Compare Map Filter", + "type": "object", + "definitions": { + "distance_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/distance_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json new file mode 100644 index 0000000000000..3c301d5ad9fbb --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Based Approximate Compare Map Filter", + "type": "object", + "definitions": { + "voxel_based_approximate_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_based_approximate_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..2e541662a1743 --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Based Compare Map Filter", + "type": "object", + "definitions": { + "voxel_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + }, + "map_update_distance_threshold": { + "type": "number", + "default": "10.0", + "description": "Threshold distance to update map points with input points [m]" + }, + "map_loader_radius": { + "type": "number", + "default": "150.0", + "description": "Radius to load map points [m]" + }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + } + }, + "required": [ + "distance_threshold", + "use_dynamic_map_loading", + "downsize_ratio_z_axis", + "map_update_distance_threshold", + "map_loader_radius", + "timer_interval_ms" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..4663dbe806223 --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Distance Based Compare Map Filter", + "type": "object", + "definitions": { + "voxel_distance_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_distance_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp index 7c8f2e8d4bd76..1eadeeb3bec05 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp @@ -40,9 +40,9 @@ CompareElevationMapFilterComponent::CompareElevationMapFilterComponent( : Filter("CompareElevationMapFilter", options) { unsubscribe(); - layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); - height_diff_thresh_ = this->declare_parameter("height_diff_thresh", 0.15); - map_frame_ = this->declare_parameter("map_frame", "map"); + layer_name_ = declare_parameter("map_layer_name"); + height_diff_thresh_ = declare_parameter("height_diff_thresh"); + map_frame_ = declare_parameter("map_frame"); rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index e38b747a6ce67..848293f6334bb 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -92,6 +92,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node boost::optional getHighestConfidenceTrafficSignal( const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const; + void removeDuplicateIds(TrafficSignalArray & signal_array) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 870b8bc7c13f5..6527f0662bcbf 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -174,6 +174,8 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output); } + removeDuplicateIds(output); + updateLastDetectedSignal(traffic_light_id_map); pub_traffic_light_array_->publish(output); @@ -383,6 +385,21 @@ boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenc return ret; } + +void CrosswalkTrafficLightEstimatorNode::removeDuplicateIds(TrafficSignalArray & signal_array) const +{ + auto & signals = signal_array.signals; + std::sort(signals.begin(), signals.end(), [](const auto & s1, const auto & s2) { + return s1.traffic_signal_id < s2.traffic_signal_id; + }); + + signals.erase( + std::unique( + signals.begin(), signals.end(), + [](const auto & s1, const auto s2) { return s1.traffic_signal_id == s2.traffic_signal_id; }), + signals.end()); +} + } // namespace traffic_light #include diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp similarity index 88% rename from perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index ae6162542a1c3..4331b5c19d3f1 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include #include @@ -69,4 +69,4 @@ class ObjectLaneletFilterNode : public rclcpp::Node } // namespace object_lanelet_filter -#endif // DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp similarity index 84% rename from perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index b9384e0f70379..be25eb6a353e6 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include #include @@ -55,4 +55,4 @@ class ObjectPositionFilterNode : public rclcpp::Node } // namespace object_position_filter -#endif // DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp similarity index 93% rename from perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp index 6597475ae297e..8f5fa054090f5 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ #include @@ -113,4 +113,4 @@ class Debugger }; } // namespace obstacle_pointcloud_based_validator -#endif // OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp similarity index 90% rename from perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index e7df2c409b18f..2d70247445043 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ -#define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ -#include "obstacle_pointcloud_based_validator/debugger.hpp" +#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" #include @@ -152,4 +154,5 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node }; } // namespace obstacle_pointcloud_based_validator -#endif // OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp similarity index 89% rename from perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index f62b0d866dc3a..5fd866d09e725 100644 --- a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ #include #include @@ -68,4 +68,4 @@ class OccupancyGridBasedValidator : public rclcpp::Node }; } // namespace occupancy_grid_based_validator -#endif // OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/include/utils/utils.hpp b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp similarity index 84% rename from perception/detected_object_validation/include/utils/utils.hpp rename to perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp index 21df77825176b..2c46e1b9dd110 100644 --- a/perception/detected_object_validation/include/utils/utils.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include @@ -33,4 +33,4 @@ struct FilterTargetLabel }; // struct FilterTargetLabel } // namespace utils -#endif // UTILS__UTILS_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index fe8de37c778ac..cd16d414425cb 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_filter/object_lanelet_filter.hpp" +#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" #include #include diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 0f59e60d57d55..a509fc7571dd5 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_filter/object_position_filter.hpp" +#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" namespace object_position_filter { diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 3249581513dd9..d903a9ca3bb41 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" #include #include diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 948f040d7ebde..80e4dc66d35c2 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" #include #include diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/src/utils.cpp index d8f438e265c3a..53b21ee7ff5b6 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 10affd0b94ffd..771747c80bb95 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -16,6 +16,7 @@ #define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ #include "detection_by_tracker/debugger.hpp" +#include "detection_by_tracker/utils.hpp" #include #include @@ -39,8 +40,6 @@ #include #endif -#include "utils/utils.hpp" - #include #include @@ -82,7 +81,7 @@ class DetectionByTracker : public rclcpp::Node std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - utils::TrackerIgnoreLabel tracker_ignore_; + detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/include/utils/utils.hpp b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp similarity index 81% rename from perception/detection_by_tracker/include/utils/utils.hpp rename to perception/detection_by_tracker/include/detection_by_tracker/utils.hpp index 3f39125b95e03..ed783b0343a14 100644 --- a/perception/detection_by_tracker/include/utils/utils.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef DETECTION_BY_TRACKER__UTILS_HPP_ +#define DETECTION_BY_TRACKER__UTILS_HPP_ #include +namespace detection_by_tracker +{ namespace utils { struct TrackerIgnoreLabel @@ -32,5 +34,6 @@ struct TrackerIgnoreLabel bool isIgnore(const uint8_t label) const; }; // struct TrackerIgnoreLabel } // namespace utils +} // namespace detection_by_tracker -#endif // UTILS__UTILS_HPP_ +#endif // DETECTION_BY_TRACKER__UTILS_HPP_ diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp index 29a500a24cf32..b2655d99abacc 100644 --- a/perception/detection_by_tracker/src/utils.cpp +++ b/perception/detection_by_tracker/src/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "detection_by_tracker/utils.hpp" #include +namespace detection_by_tracker +{ namespace utils { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -28,3 +30,4 @@ bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } } // namespace utils +} // namespace detection_by_tracker diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index e1be5426cba4b..21d31f216373b 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,18 +1,26 @@ /**: ros__parameters: - class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] - paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] - point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle - max_voxel_size: 40000 - point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] - voxel_size: [0.32, 0.32, 8.0] - downsample_factor: 1 - encoder_in_feature_size: 12 - yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] - # post-process params - circle_nms_dist_threshold: 0.3 - iou_nms_target_class_names: ["CAR"] - iou_nms_search_distance_2d: 10.0 - iou_nms_threshold: 0.1 - # omp params - omp_num_threads: 1 + model_params: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle + max_voxel_size: 40000 + point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + voxel_size: [0.32, 0.32, 8.0] + downsample_factor: 1 + encoder_in_feature_size: 12 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + has_twist: false + densification_params: + world_frame_id: "map" + num_past_frames: 0 + post_process_params: + # post-process params + circle_nms_dist_threshold: 0.3 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + score_threshold: 0.4 + omp_params: + # omp params + num_threads: 1 diff --git a/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml new file mode 100644 index 0000000000000..90ba841d53b2d --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + fusion_distance: 100.0 + trust_object_distance: 100.0 + trust_object_iou_mode: "iou" + non_trust_object_iou_mode: "iou_x" + use_cluster_semantic_type: false + only_allow_inside_cluster: true + roi_scale_factor: 1.1 + iou_threshold: 0.65 + unknown_iou_threshold: 0.1 + remove_unknown: true diff --git a/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..5b86b8e81d1aa --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + fuse_unknown_only: true + min_cluster_size: 2 + cluster_2d_tolerance: 0.5 diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml index 21ba13787f1c0..99d85089befb8 100644 --- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml +++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml @@ -3,3 +3,11 @@ input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] timeout_ms: 70.0 match_threshold_ms: 50.0 + image_buffer_size: 15 + debug_mode: false + filter_scope_min_x: -100.0 + filter_scope_min_y: -100.0 + filter_scope_min_z: -100.0 + filter_scope_max_x: 100.0 + filter_scope_max_y: 100.0 + filter_scope_max_z: 100.0 diff --git a/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg new file mode 100644 index 0000000000000..aaadfaf186dd2 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg @@ -0,0 +1,966 @@ + + + + + + + + + + +
+
+
+ + Check whether + the cluster RoI (inner) + is fully contained within + the scaled detected RoI (outer) + . +
+
+ *NOTE* +
+ Because the cluster RoI is calculated with the points projected on the image, our recommended scale factor is [1.1, 1.5] . +
+ Otherwise, set + only_allow_inside_cluster + to + False + . +
+ +
+
+
+ + Check whether the cluster RoI (inner) is fully contained within the scaled... + + + + + + +
+
+
+ use_iou +
+
+
+ + use_iou + + + + + + +
+
+
+ unknown_iou_threshold +
+
+
+ + unknown_iou_threshold + + + + + + +
+
+
+ + use_iou_x /  y +
+ +
+
+
+ + use_iou_x /  y + + + + + + + + +
+
+
+ only_allow_inside_cluster +
+
+
+ + only_allow_inside_cluster + + + + + + +
+
+
+ iou_threshold +
+
+
+ + iou_threshold + + + + + + + + + + +
+
+
+ Is UNKNOWN object? +
+
+
+ + Is UNKNOWN object? + + + + + + + + +
+
+
+ + IoU threshold < IoU score? + +
+
+
+ + IoU threshold < IoU score? + + + + + + + + + + + + + + +
+
+
+ IoU Score +
+
+
+ + IoU Score + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Is Cluster RoI inside Detected RoI? +
+
+
+ + Is Cluster RoI inside Detected RoI? + + + + + + + + +
+
+
+ Fused Cluster +
+
+
+ + Fused Cluster + + + + + + + + +
+
+
+ Unfused Cluster +
+
+
+ + Unfused Cluster + + + + + + + + + + +
+
+
+ Fusion Target Cluster
&
Detected RoI +
+
+
+ + Fusion Target Cluster... + + + + + + +
+
+
+ Unfused Cluster +
+
+
+ + Unfused Cluster + + + + + + + + + + + + + +
+
+
+ + + roi_scale_factor
(>=1.0) + + +
+
+
+ + roi_scale_factor... + + + + + + +
+
+
+ + Check whether the IoU score between + the cluster RoI + and + the detected RoI + is greater than the threshold. +
+
+ **NOTE** +
+ Our default threshold is 0.65 with + only_allow_inside_cluster + is + True + . +
+ Also, please try the bigger score threshold and set + only_allow_inside_cluster + to + False + depending on your 2D RoI detector performance. +
+ +
+
+
+ + Check whether the IoU score between the cluster RoI and the detected RoI is greater than the thr... + + + + + + + + +
+
+
+
    +
  • + IoU = Intersection Area / Union Area +
    • +
  • + IoU X = Intersection Width / Union Width +
    • +
  • + IoU Y = Intersection Height / Union Height +
    • +
+
+
+
+ + IoU = Intersection Area / Union AreaIoU X = Intersectio... + + + + + + +
+
+
+ + Nodes overview + +
+
+
+ + Nodes overview + + + + + + +
+
+
+ Numeric Parameter +
+
+
+ + Numeric Parameter + + + + + + +
+
+
+ Result +
+
+
+ + Result + + + + + + + + + + +
+
+
+ Boolean Parameter +
+
+
+ + Boolean Parameter + + + + + + +
+
+
+ + true + +
+
+
+ + true + + + + + + +
+
+
+ + false + +
+
+
+ + false + + + + + + + + + + +
+
+
+ Condition +
+
+
+ + Condition + + + + + + +
+
+
+ + true + +
+
+
+ + true + + + + + + +
+
+
+ + false + +
+
+
+ + false + + + + + + +
+
+
+ use_cluster_semantic_type +
+
+
+ + use_cluster_semantic_type + + + + + + +
+
+
+ + Objects +
+ w/ Semantic Label + +
+
+
+ + Objects... + + + + + + + + +
+
+
+ + All objects are set to +
+ UNKOWN Label +
+ +
+
+
+ + All objects are set to... + + + + + + + + + + +
+
+
+ Fusion Target Cluster +
+
+
+ + Fusion Target Cluster + + + + + + +
+
+
+ trust_distance +
+
+
+ + trust_distance + + + + + + + + +
+
+
+ + Is Cluster closer than + trust_distance + ? + +
+
+
+ + Is Cluster closer than trust_... + + + + + + + + + + + + + + +
+
+
+ + Input Cluster + +
+
+
+ + Input Cluster + + + + + + +
+
+
+ roi_scale_factor +
+
+
+ + roi_scale_factor + + + + + + +
+
+
+ Pre-Process +
+
+
+ + Pre-Process + + + + + + +
+
+
+ Fusion Process +
+
+
+ + Fusion Process + + + + + + +
+
+
+ Post-Process +
+
+
+ + Post-Process + + + + + + + + +
+
+
+ remove_unknown +
+
+
+ + remove_unknown + + + + + + + + +
+
+
+ Fused Cluster +
+
+
+ + Fused Cluster + + + + + + +
+
+
+ Is KNOWN object?
&&
0.1 <= RoI probability?
 +
+
+
+ + Is KNOWN object?... + + + + + + + + +
+
+
+ Remove noise clusters, which are undetected by RoI detector, such as fog and exhaust gas. +
+
+
+ + Remove noise clusters, which are undet... + + + + + + +
+
+
+ + Output Fused Cluster + +
+
+
+ + Output Fused Cluster + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 03eaab2a3c6ca..86d3a2fa070b2 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -30,6 +30,11 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ## Parameters +The following figure is an inner pipeline overview of RoI cluster fusion node. +Please refer to it for your parameter settings. + +![roi_cluster_fusion_pipeline](./images/roi_cluster_fusion_pipeline.svg) + ### Core Parameters | Name | Type | Description | diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index e15737f5ed222..33781461fa1cc 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -30,7 +30,6 @@ - @@ -43,9 +42,6 @@ - - - @@ -82,9 +78,6 @@ - - - diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 60f6f943b8cda..52dd71e9579c1 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -18,24 +18,11 @@ + - - - - - - - - - - - - - - @@ -46,17 +33,8 @@ - - - - - - - - - - + @@ -86,16 +64,6 @@ - - - - - - - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index b6165fc7c87d2..c9da81af9ddb0 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -25,8 +25,6 @@ - - @@ -69,10 +67,6 @@ - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index 181f4ccb88320..046d88d06e2a1 100644 --- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -1,8 +1,5 @@ - - - @@ -23,6 +20,7 @@ + @@ -38,9 +36,7 @@ - - - + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 1648de210ec2c..49ff4dafc7900 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -9,6 +9,8 @@ Shunsuke Miura Yoshi Ri badai nguyen + Kotaro Uetake + Tao Zhong Apache License 2.0 ament_cmake_auto diff --git a/perception/image_projection_based_fusion/schema/pointpainting.schema.json b/perception/image_projection_based_fusion/schema/pointpainting.schema.json new file mode 100644 index 0000000000000..036628d72e70a --- /dev/null +++ b/perception/image_projection_based_fusion/schema/pointpainting.schema.json @@ -0,0 +1,152 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Point Painting Fusion Node", + "type": "object", + "definitions": { + "pointpainting": { + "type": "object", + "properties": { + "model_params": { + "type": "object", + "description": "Parameters for model configuration.", + "properties": { + "class_names": { + "type": "array", + "description": "An array of class names will be predicted.", + "default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "paint_class_names": { + "type": "array", + "description": "An array of class names will be painted by PointPainting", + "default": ["CAR", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "point_feature_size": { + "type": "integer", + "description": "A number of channels of point feature layer.", + "default": 7 + }, + "max_voxel_size": { + "type": "integer", + "description": "A maximum size of voxel grid.", + "default": 40000 + }, + "point_cloud_range": { + "type": "array", + "description": "An array of distance ranges of each class, this must have same length with `class_names`.", + "default": [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + }, + "voxel_size": { + "type": "array", + "description": "An array of voxel grid sizes for PointPainting, this must have same length with `paint_class_names`.", + "default": [0.32, 0.32, 8.0] + }, + "down_sample_factor": { + "type": "integer", + "description": "A scale factor of downsampling points", + "default": 1, + "minimum": 1 + }, + "encoder_in_feature_size": { + "type": "integer", + "description": "A size of encoder input feature channels.", + "default": 12 + }, + "yaw_norm_thresholds": { + "type": "array", + "description": "An array of distance threshold values of norm of yaw [rad].", + "default": [0.3, 0.3, 0.3, 0.3, 0.0], + "minimum": 0.0, + "maximum": 1.0 + }, + "has_twist": { + "type": "boolean", + "description": "Indicates whether the model outputs twist value.", + "default": false + } + } + }, + "densification_params": { + "type": "object", + "description": "Parameters for pointcloud densification.", + "properties": { + "world_frame_id": { + "type": "string", + "description": "A name of frame id where world coordinates system is defined with respect to.", + "default": "map" + }, + "num_past_frames": { + "type": "integer", + "description": "A number of past frames to be considered as same input frame.", + "default": 0, + "minimum": 0 + } + } + }, + "post_process_params": { + "type": "object", + "properties": { + "score_threshold": { + "type": "number", + "description": "A threshold value of existence probability score, all of objects with score less than this threshold are ignored.", + "default": 0.4, + "minimum": 0.0, + "maximum": 1.0 + }, + "circle_nms_dist_threshold": { + "type": "number", + "description": "", + "default": 0.3, + "minimum": 0.0, + "maximum": 1.0 + }, + "iou_nms_target_class_names": { + "type": "array", + "description": "An array of class names to be target in NMS.", + "default": ["CAR"], + "uniqueItems": true + }, + "iou_search_distance_2d": { + "type": "number", + "description": "A maximum distance value to search the nearest objects.", + "default": 10.0, + "minimum": 0.0 + }, + "iou_nms_threshold": { + "type": "number", + "description": "A threshold value of NMS using IoU score.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + } + } + }, + "omp_params": { + "type": "object", + "properties": { + "num_threads": { + "type": "integer", + "description": "The number of threads that is set to the environment variable OMP_NUM_THREADS.", + "default": 1, + "minimum": 1 + } + } + } + }, + "required": ["model_params", "densification_params", "post_process_params", "omp_params"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pointpainting" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json new file mode 100644 index 0000000000000..fc32e9d6d3d8b --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json @@ -0,0 +1,96 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI Cluster Fusion Node", + "type": "object", + "definitions": { + "roi_cluster_fusion": { + "type": "object", + "properties": { + "fusion_distance": { + "type": "number", + "description": "If the detected object's distance is less than its value, the fusion will be processed.", + "default": 100.0, + "minimum": 0.0 + }, + "trust_object_distance": { + "type": "number", + "description": "If the detected object's distance is less than its value, IoU method specified in `trust_object_iou_mode` is used, otherwise `non_trust_object_iou_mode` is used.", + "default": 100.0, + "minimum": 0.0 + }, + "trust_object_iou_mode": { + "type": "string", + "description": "Name of IoU method applied to the objects in range of [0.0, `trust_distance`].", + "default": "iou", + "enum": ["iou", "iou_x", "iou_y"] + }, + "non_trust_object_iou_mode": { + "type": "string", + "description": "Name of IoU method applied to the objects in range of [`trust_distance`, `fusion_distance`], if `trust_distance` < `fusion_distance`.", + "default": "iou_x", + "enum": ["iou", "iou_x", "iou_y"] + }, + "use_cluster_semantic_type": { + "type": "boolean", + "description": "If this parameter is false, label of cluster objects will be reset to UNKNOWN.", + "default": false + }, + "only_allow_inside_cluster": { + "type": "boolean", + "description": "If this parameter is true, only clusters in which all their points are inside the RoI can be assigned to the RoI.", + "default": true + }, + "roi_scale_factor": { + "type": "number", + "description": "A scale factor for resizing RoI while checking if cluster points are inside the RoI.", + "default": 1.1, + "minimum": 1.0, + "maximum": 2.0 + }, + "iou_threshold": { + "type": "number", + "description": "An IoU score threshold. Note that the total IoU score is the sum of the IoU scores that are set to true in use_iou, use_iou_x and use_iou_y.", + "default": 0.65, + "minimum": 0.0, + "maximum": 1.0 + }, + "unknown_iou_threshold": { + "type": "number", + "description": "A threshold value of IoU score for objects labeled UNKNOWN.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + }, + "remove_unknown": { + "type": "boolean", + "description": "If this parameter is true, all of objects labeled UNKNOWN will be removed in post-process.", + "default": false + } + }, + "required": [ + "fusion_distance", + "trust_object_distance", + "trust_object_iou_mode", + "non_trust_object_iou_mode", + "use_cluster_semantic_type", + "only_allow_inside_cluster", + "roi_scale_factor", + "iou_threshold", + "unknown_iou_threshold", + "remove_unknown" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_cluster_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json new file mode 100644 index 0000000000000..3030be1305d56 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json @@ -0,0 +1,70 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI Detected Object Fusion Node", + "type": "object", + "definitions": { + "roi_detected_object_fusion": { + "type": "object", + "properties": { + "passthrough_lower_bound_probability_thresholds": { + "type": "array", + "description": "An array of object probability thresholds. The objects that have higher probability than their respective thresholds are kept.", + "default": [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.5] + }, + "trust_distances": { + "type": "array", + "description": "An array of object distances thresholds. Any objects that is farther than this value will be skipped in the clustering process, but will still be published.", + "default": [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + }, + "min_iou_threshold": { + "type": "number", + "description": "An Iou score threshold.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + }, + "roi_probability_threshold": { + "type": "number", + "description": "A object probability threshold.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + }, + "use_roi_probability": { + "type": "boolean", + "description": "If this parameter is true, the objects are filtered out with their RoI probabilities.", + "default": false + }, + "can_assign_matrix": { + "type": "array", + "description": "An NxN matrix, where N represents the number of classes. A value 1 indicates that it is assignable, while a value of 0 indicates not.", + "default": [ + 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, + 0, 0, 0, 1, 1, 1 + ] + } + }, + "required": [ + "passthrough_lower_bound_probability_thresholds", + "trust_distances", + "min_iou_threshold", + "roi_probability_threshold", + "use_roi_probability", + "can_assign_matrix" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_detected_object_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..f39ef257ea789 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json @@ -0,0 +1,41 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI PointCloud Fusion Node", + "type": "object", + "definitions": { + "roi_pointcloud_fusion": { + "type": "object", + "properties": { + "fuse_unknown_only": { + "type": "boolean", + "description": "Whether to fuse only UNKNOWN clusters.", + "default": true + }, + "min_cluster_size": { + "type": "integer", + "description": "The minimum number of points that a cluster must contain to be considered as valid.", + "default": 2 + }, + "cluster_2d_tolerance": { + "type": "number", + "description": "A cluster tolerance measured in radial direction [m]", + "default": 0.5, + "exclusiveMinimum": 0.0 + } + }, + "required": ["fuse_unknown_only", "min_cluster_size", "cluster_2d_tolerance"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_sync.schema.json b/perception/image_projection_based_fusion/schema/roi_sync.schema.json new file mode 100644 index 0000000000000..411fb678a49a7 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_sync.schema.json @@ -0,0 +1,84 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Synchronization of RoI Fusion Nodes", + "type": "object", + "definitions": { + "roi_sync": { + "type": "object", + "properties": { + "input_offset_ms": { + "type": "array", + "description": "An array of timestamp offsets for each camera [ms].", + "default": [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + }, + "timeout_ms": { + "type": "number", + "description": "A timeout value can be assigned within a single frame [ms].", + "default": 70.0, + "minimum": 1.0, + "maximum": 100.0 + }, + "match_threshold_ms": { + "type": "number", + "description": "A maximum threshold value to synchronize RoIs from multiple cameras [ms].", + "default": 50.0, + "minimum": 0.0, + "maximum": 100.0 + }, + "image_buffer_size": { + "type": "integer", + "description": "The number of image buffer size for debug.", + "default": 15, + "minimum": 1 + }, + "debug_mode": { + "type": "boolean", + "description": "Whether to debug or not.", + "default": false + }, + "filter_scope_min_x": { + "type": "number", + "description": "Minimum x position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_min_y": { + "type": "number", + "description": "Minimum y position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_min_z": { + "type": "number", + "description": "Minimum z position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_max_x": { + "type": "number", + "description": "Maximum x position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_max_y": { + "type": "number", + "description": "Maximum y position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_max_z": { + "type": "number", + "description": "Maximum z position [m].", + "default": 100.0 + } + } + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_sync" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index b01a910aaded1..4abd92d7fb063 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -46,7 +46,7 @@ FusionNode::FusionNode( : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number", 1)); + rois_number_ = static_cast(declare_parameter("rois_number")); if (rois_number_ < 1) { RCLCPP_WARN( this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); @@ -80,7 +80,7 @@ FusionNode::FusionNode( "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - input_offset_ms_ = declare_parameter("input_offset_ms", std::vector{}); + input_offset_ms_ = declare_parameter>("input_offset_ms"); if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -122,7 +122,7 @@ FusionNode::FusionNode( // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = - static_cast(declare_parameter("image_buffer_size", 15)); + static_cast(declare_parameter("image_buffer_size")); debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } @@ -136,14 +136,15 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } + // cspell: ignore minx, maxx, miny, maxy, minz, maxz // FIXME: use min_x instead of minx - filter_scope_minx_ = declare_parameter("filter_scope_minx", -100); - filter_scope_maxx_ = declare_parameter("filter_scope_maxx", 100); - filter_scope_miny_ = declare_parameter("filter_scope_miny", -100); - filter_scope_maxy_ = declare_parameter("filter_scope_maxy", 100); - filter_scope_minz_ = declare_parameter("filter_scope_minz", -100); - filter_scope_maxz_ = declare_parameter("filter_scope_maxz", 100); + filter_scope_minx_ = declare_parameter("filter_scope_min_x"); + filter_scope_maxx_ = declare_parameter("filter_scope_max_x"); + filter_scope_miny_ = declare_parameter("filter_scope_min_y"); + filter_scope_maxy_ = declare_parameter("filter_scope_max_y"); + filter_scope_minz_ = declare_parameter("filter_scope_min_z"); + filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); } template diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 48ef3d26806c8..76b561f677c0f 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -95,28 +95,29 @@ inline bool isUnknown(int label2d) PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) : FusionNode("pointpainting_fusion", options) { - omp_num_threads_ = this->declare_parameter("omp_num_threads", 1); + omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = - static_cast(this->declare_parameter("score_threshold", 0.4)); - const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + static_cast(this->declare_parameter("post_process_params.score_threshold")); + const float circle_nms_dist_threshold = static_cast( + this->declare_parameter("post_process_params.circle_nms_dist_threshold")); const auto yaw_norm_thresholds = - this->declare_parameter>("yaw_norm_thresholds"); + this->declare_parameter>("model_params.yaw_norm_thresholds"); // densification param const std::string densification_world_frame_id = - this->declare_parameter("densification_world_frame_id", "map"); + this->declare_parameter("densification_params.world_frame_id"); const int densification_num_past_frames = - this->declare_parameter("densification_num_past_frames", 0); + this->declare_parameter("densification_params.num_past_frames"); // network param - const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); - const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path", ""); - const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path", ""); - const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); - const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); - - class_names_ = this->declare_parameter>("class_names"); + const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); + const std::string encoder_engine_path = + this->declare_parameter("encoder_engine_path"); + const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); + const std::string head_engine_path = this->declare_parameter("head_engine_path"); + + class_names_ = this->declare_parameter>("model_params.class_names"); const auto paint_class_names = - this->declare_parameter>("paint_class_names"); + this->declare_parameter>("model_params.paint_class_names"); std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; if ( std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") != @@ -138,17 +139,17 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_.erase(cls); } } - has_twist_ = this->declare_parameter("has_twist", false); - const std::size_t point_feature_size = - static_cast(this->declare_parameter("point_feature_size")); + has_twist_ = this->declare_parameter("model_params.has_twist"); + const std::size_t point_feature_size = static_cast( + this->declare_parameter("model_params.point_feature_size")); const std::size_t max_voxel_size = - static_cast(this->declare_parameter("max_voxel_size")); - pointcloud_range = this->declare_parameter>("point_cloud_range"); - const auto voxel_size = this->declare_parameter>("voxel_size"); - const std::size_t downsample_factor = - static_cast(this->declare_parameter("downsample_factor")); - const std::size_t encoder_in_feature_size = - static_cast(this->declare_parameter("encoder_in_feature_size")); + static_cast(this->declare_parameter("model_params.max_voxel_size")); + pointcloud_range = this->declare_parameter>("model_params.point_cloud_range"); + const auto voxel_size = this->declare_parameter>("model_params.voxel_size"); + const std::size_t downsample_factor = static_cast( + this->declare_parameter("model_params.downsample_factor")); + const std::size_t encoder_in_feature_size = static_cast( + this->declare_parameter("model_params.encoder_in_feature_size")); const auto allow_remapping_by_area_matrix = this->declare_parameter>("allow_remapping_by_area_matrix"); const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); @@ -172,10 +173,11 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt { centerpoint::NMSParams p; p.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; - p.target_class_names_ = - this->declare_parameter>("iou_nms_target_class_names"); - p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); - p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + p.target_class_names_ = this->declare_parameter>( + "post_process_params.iou_nms_target_class_names"); + p.search_distance_2d_ = + this->declare_parameter("post_process_params.iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("post_process_params.iou_nms_threshold"); iou_bev_nms_.setParameters(p); } diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index 0cc8f1708e334..311fe7b6da716 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -70,16 +70,26 @@ Example: ### Core Parameters -| Name | Type | Description | -| --------------------------- | ------ | -------------------------------------------------------------- | -| `can_assign_matrix` | double | Assignment table for data association | -| `max_dist_matrix` | double | Maximum distance table for data association | -| `max_area_matrix` | double | Maximum area table for data association | -| `min_area_matrix` | double | Minimum area table for data association | -| `max_rad_matrix` | double | Maximum angle table for data association | -| `world_frame_id` | double | tracking frame | -| `enable_delay_compensation` | bool | Estimate obstacles at current time considering detection delay | -| `publish_rate` | double | if enable_delay_compensation is true, how many hertz to output | +Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_object_tracker.param.yaml) and association parameters are defined in [data_association.param.yaml](config/data_association.param.yaml). + +#### Node parameters + +| Name | Type | Description | +| --------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | +| `***_tracker` | string | EKF tracker name for each class | +| `world_frame_id` | double | object kinematics definition frame | +| `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | +| `publish_rate` | double | Timer frequency to output with delay compensation | + +#### Association parameters + +| Name | Type | Description | +| ------------------- | ------ | ------------------------------------------- | +| `can_assign_matrix` | double | Assignment table for data association | +| `max_dist_matrix` | double | Maximum distance table for data association | +| `max_area_matrix` | double | Maximum area table for data association | +| `min_area_matrix` | double | Minimum area table for data association | +| `max_rad_matrix` | double | Maximum angle table for data association | ## Assumptions / Known limits diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml similarity index 50% rename from perception/multi_object_tracker/config/default_tracker.param.yaml rename to perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index 23e88d8274172..ca320c7f58442 100644 --- a/perception/multi_object_tracker/config/default_tracker.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + # default tracker models for each class car_tracker: "multi_vehicle_tracker" truck_tracker: "multi_vehicle_tracker" bus_tracker: "multi_vehicle_tracker" @@ -7,3 +8,14 @@ pedestrian_tracker: "pedestrian_and_bicycle_tracker" bicycle_tracker: "pedestrian_and_bicycle_tracker" motorcycle_tracker: "pedestrian_and_bicycle_tracker" + + # default tracker node parameters + publish_rate: 10.0 + world_frame_id: map + enable_delay_compensation: false + + # debug parameters + publish_processing_time: false + publish_tentative_objects: false + diagnostics_warn_delay: 0.5 # [sec] + diagnostics_error_delay: 1.0 # [sec] diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index eb8607b91b50c..95d33b78ff184 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -23,6 +23,8 @@ #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +#include +#include #include #include @@ -38,6 +40,9 @@ #include #endif +#include +#include + #include #include @@ -47,6 +52,41 @@ #include #include +/** + * @brief Debugger class for multi object tracker + * @details This class is used to publish debug information of multi object tracker + */ +class TrackerDebugger +{ +public: + explicit TrackerDebugger(rclcpp::Node & node); + void publishProcessingTime(); + void publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void startStopWatch(); + void startMeasurementTime(const rclcpp::Time & measurement_header_stamp); + void setupDiagnostics(); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + struct DEBUG_SETTINGS + { + bool publish_processing_time; + bool publish_tentative_objects; + double diagnostics_warn_delay; + double diagnostics_error_delay; + } debug_settings_; + double elapsed_time_from_sensor_input_ = 0.0; + diagnostic_updater::Updater diagnostic_updater_; + +private: + void loadParameters(); + rclcpp::Node & node_; + rclcpp::Publisher::SharedPtr + debug_tentative_objects_pub_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + rclcpp::Time last_input_stamp_; +}; + class MultiObjectTracker : public rclcpp::Node { public: @@ -59,6 +99,9 @@ class MultiObjectTracker : public rclcpp::Node detected_object_sub_; rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + // debugger class + std::unique_ptr debugger_; + tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -81,7 +124,7 @@ class MultiObjectTracker : public rclcpp::Node const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; - void publish(const rclcpp::Time & time) const; + void publish(const rclcpp::Time & time); inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index 21a2ce67d552d..dcd080b851c20 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -2,18 +2,12 @@ - - - - + - - - diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d302b04d733cd..d28833241bd5f 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -65,6 +65,113 @@ boost::optional getTransformAnonymous( } // namespace +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) +: diagnostic_updater_(&node), node_(node), last_input_stamp_(node.now()) +{ + // declare debug parameters to decide whether to publish debug topics + loadParameters(); + // initialize debug publishers + stop_watch_ptr_ = std::make_unique>(); + if (debug_settings_.publish_processing_time) { + processing_time_publisher_ = + std::make_unique(&node_, "multi_object_tracker"); + } + + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_ = + node_.create_publisher( + "debug/tentative_objects", rclcpp::QoS{1}); + } + + // initialize stop watch and diagnostics + startStopWatch(); + setupDiagnostics(); +} + +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); +} + +void TrackerDebugger::loadParameters() +{ + try { + debug_settings_.publish_processing_time = + node_.declare_parameter("publish_processing_time"); + debug_settings_.publish_tentative_objects = + node_.declare_parameter("publish_tentative_objects"); + debug_settings_.diagnostics_warn_delay = + node_.declare_parameter("diagnostics_warn_delay"); + debug_settings_.diagnostics_error_delay = + node_.declare_parameter("diagnostics_error_delay"); + } catch (const std::exception & e) { + RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); + debug_settings_.publish_processing_time = false; + debug_settings_.publish_tentative_objects = false; + debug_settings_.diagnostics_warn_delay = 0.5; + debug_settings_.diagnostics_error_delay = 1.0; + } +} + +void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + const double delay = elapsed_time_from_sensor_input_; // [s] + + if (delay == 0.0) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); + } else if (delay < debug_settings_.diagnostics_warn_delay) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); + } else if (delay < debug_settings_.diagnostics_error_delay) { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); + } else { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); + } + + stat.add("Detection delay", delay); +} + +void TrackerDebugger::publishProcessingTime() +{ + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const auto current_time = node_.now(); + elapsed_time_from_sensor_input_ = (current_time - last_input_stamp_).seconds(); + if (debug_settings_.publish_processing_time) { + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + processing_time_publisher_->publish( + "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + } +} + +void TrackerDebugger::publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +void TrackerDebugger::startStopWatch() +{ + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); +} + +void TrackerDebugger::startMeasurementTime(const rclcpp::Time & measurement_header_stamp) +{ + last_input_stamp_ = measurement_header_stamp; + // start measuring processing time + stop_watch_ptr_->toc("processing_time", true); +} + MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), @@ -78,9 +185,12 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) create_publisher("output", rclcpp::QoS{1}); // Parameters - double publish_rate = declare_parameter("publish_rate", 30.0); - world_frame_id_ = declare_parameter("world_frame_id", "world"); - bool enable_delay_compensation{declare_parameter("enable_delay_compensation", false)}; + double publish_rate = declare_parameter("publish_rate"); + world_frame_id_ = declare_parameter("world_frame_id"); + bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + + // Debug publishers + debugger_ = std::make_unique(*this); auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); @@ -126,6 +236,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { + /* keep the latest input stamp and check transform*/ + debugger_->startMeasurementTime(rclcpp::Time(input_objects_msg->header.stamp)); const auto self_transform = getTransformAnonymous( tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); if (!self_transform) { @@ -324,7 +436,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish( return true; } -void MultiObjectTracker::publish(const rclcpp::Time & time) const +void MultiObjectTracker::publish(const rclcpp::Time & time) { const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); @@ -332,11 +444,16 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - autoware_auto_perception_msgs::msg::TrackedObjects output_msg; + autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; output_msg.header.stamp = time; + tentative_objects_msg.header = output_msg.header; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - if (!shouldTrackerPublish(*itr)) { + if (!shouldTrackerPublish(*itr)) { // for debug purpose + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + tentative_objects_msg.objects.push_back(object); continue; } autoware_auto_perception_msgs::msg::TrackedObject object; @@ -346,6 +463,10 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const // Publish tracked_objects_pub_->publish(output_msg); + + // Debugger Publish if enabled + debugger_->publishProcessingTime(); + debugger_->publishTentativeObjects(tentative_objects_msg); } RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index 6e6813d3e40ff..60ed2efb5767b 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -2,9 +2,6 @@ - - - @@ -19,8 +16,5 @@ - - - diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp new file mode 100644 index 0000000000000..ec1a2b8a42973 --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp @@ -0,0 +1,40 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#define SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ + +#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "utils.hpp" + +class BicycleCorrector : public VehicleCorrector +{ +public: + explicit BicycleCorrector(const bool use_reference_yaw = false) + : VehicleCorrector(use_reference_yaw) + { + corrector_utils::CorrectionBBParameters params; + params.min_width = 0.3; + params.max_width = 1.2; + params.default_width = (params.min_width + params.max_width) * 0.5; + params.min_length = 1.0; + params.max_length = 2.8; + params.default_length = (params.min_length + params.max_length) * 0.5; + setParams(params); + } + + ~BicycleCorrector() = default; +}; + +#endif // SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp index d52bdc21f916f..e4efc17181e52 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp @@ -15,6 +15,7 @@ #ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ #define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ +#include "shape_estimation/corrector/bicycle_corrector.hpp" #include "shape_estimation/corrector/bus_corrector.hpp" #include "shape_estimation/corrector/car_corrector.hpp" #include "shape_estimation/corrector/corrector_interface.hpp" diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 9577b1e2d3197..e89eaac0a6db0 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -120,6 +120,8 @@ bool ShapeEstimator::applyCorrector( corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); } else if (label == Label::TRAILER) { corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); + } else if (label == Label::MOTORCYCLE || label == Label::BICYCLE) { + corrector_ptr.reset(new BicycleCorrector(use_reference_yaw)); } else { corrector_ptr.reset(new NoCorrector); } diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp index bc6dfef9b80bf..951be5d45d508 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -18,6 +18,8 @@ #define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ // #include +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 5987bdc2d1bef..81f63538f9e22 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -253,6 +253,68 @@ double mean(const double a, const double b) return (a + b) / 2.0; } +/** + * @brief compare two tracked objects motion direction is same or not + * + * @param main_obj + * @param sub_obj + * @return true + * @return false + */ +bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + // get yaw + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + // get velocity + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj.kinematics.twist_with_covariance.twist.linear.y; + // calc velocity direction + const auto main_vyaw = std::atan2(main_vy, main_vx); + const auto sub_vyaw = std::atan2(sub_vy, sub_vx); + // get motion yaw angle + const auto main_motion_yaw = main_yaw + main_vyaw; + const auto sub_motion_yaw = sub_yaw + sub_vyaw; + // diff of motion yaw angle + const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); + const auto normalized_motion_yaw_diff = + tier4_autoware_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + // evaluate if motion yaw angle is same + constexpr double yaw_threshold = M_PI / 4.0; // 45 deg + if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { + return true; + } else { + return false; + } +} + +/** + * @brief compare two tracked objects yaw is reverted or not + * + * @param main_obj + * @param sub_obj + * @return true + * @return false + */ +bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + // get yaw + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + // calc yaw diff + const auto yaw_diff = std::fabs(main_yaw - sub_yaw); + const auto normalized_yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_diff); // -pi ~ pi + // evaluate if yaw is reverted + constexpr double yaw_threshold = M_PI / 2.0; // 90 deg + if (std::abs(normalized_yaw_diff) >= yaw_threshold) { + return true; + } else { + return false; + } +} + // object kinematics merger // currently only support velocity fusion autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( @@ -261,18 +323,29 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; // copy main object at first output_kinematics = main_obj.kinematics; + auto sub_obj_ = sub_obj; + // do not merge reverse direction objects + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + return output_kinematics; + } else if (objectsYawIsReverted(main_obj, sub_obj)) { + // revert velocity (revert pose is not necessary because it is not used in tracking) + sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0; + } + + // currently only merge vx if (policy == MergePolicy::SKIP) { return output_kinematics; } else if (policy == MergePolicy::OVERWRITE) { output_kinematics.twist_with_covariance.twist.linear.x = - sub_obj.kinematics.twist_with_covariance.twist.linear.x; + sub_obj_.kinematics.twist_with_covariance.twist.linear.x; return output_kinematics; } else if (policy == MergePolicy::FUSION) { const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; - const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; // inverse weight const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; - const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0]; double main_vx_weight, sub_vx_weight; if (main_vx_cov == 0.0) { main_vx_weight = 1.0 * 1e6; @@ -380,9 +453,21 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger( void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { - const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + // do not update velocity + return; + } else if (objectsYawIsReverted(main_obj, sub_obj)) { + // revert velocity (revert pose is not necessary because it is not used in tracking) + const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp; + return; + } else { + // update velocity + const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; + } } void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) @@ -397,6 +482,12 @@ void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & su void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) { + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + // warning + std::cerr << "[object_tracking_merger]: motion direction is different in " + "updateWholeTrackedObject function." + << std::endl; + } main_obj = sub_obj; } diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp similarity index 88% rename from perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp rename to perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp index 3f83cf6e926ad..5e981bbeafda1 100644 --- a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ -#define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include #include @@ -49,4 +49,4 @@ class TrafficLightMapVisualizerNode : public rclcpp::Node } // namespace traffic_light -#endif // TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ diff --git a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp similarity index 95% rename from perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp rename to perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp index 1c64eb2f9e82b..13d9045e89e98 100644 --- a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ -#define TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ #include #include @@ -123,4 +123,4 @@ class TrafficLightRoiVisualizerNodelet : public rclcpp::Node } // namespace traffic_light -#endif // TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp index 02f7a67302945..62d416457c6f0 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" + #include -#include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index 53e06e47a1873..e621a20450bbf 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" + #include #include #include -#include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index 2f2d80ba43233..4ff9729695be2 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp" // NOLINT(whitespace/line_length) + #include #include -#include #include #include diff --git a/planning/.pages b/planning/.pages index 7cbd36d2ca2d0..6ae8abe240d9b 100644 --- a/planning/.pages +++ b/planning/.pages @@ -14,7 +14,7 @@ nav: - 'Avoidance by Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design - 'Goal Planner': planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design - - 'Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design + - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design - 'Start Planner': planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design - 'Behavior Velocity Planner': diff --git a/planning/README.md b/planning/README.md index 4f13bac3392a0..0302d37a1180b 100644 --- a/planning/README.md +++ b/planning/README.md @@ -43,7 +43,7 @@ There are multiple parameters available for configuration, and users have the op This guide outlines the steps for integrating your custom module into Autoware: -- Add your modules to the `default_present.yaml` file. For example +- Add your modules to the `default_preset.yaml` file. For example ```yaml - arg: diff --git a/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt b/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..bca909c7cef7e --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_avoidance_by_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp + src/interface.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml similarity index 91% rename from planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml rename to planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml index 9518185d30d63..3e8907cdccdf6 100644 --- a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml @@ -85,3 +85,11 @@ bicycle: false # [-] motorcycle: false # [-] pedestrian: false # [-] + + constraints: + # lateral constraints + lateral: + velocity: [1.0, 1.38, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp new file mode 100644 index 0000000000000..8e7d1f67d3157 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -0,0 +1,35 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ + +#include "behavior_path_avoidance_module/data_structs.hpp" + +namespace behavior_path_planner +{ +struct AvoidanceByLCParameters : public AvoidanceParameters +{ + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; + + explicit AvoidanceByLCParameters(const AvoidanceParameters & param) : AvoidanceParameters(param) + { + } +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp new file mode 100644 index 0000000000000..897956a392008 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -0,0 +1,50 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/interface.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); + + bool isExecutionRequested() const override; + + void processOnEntry() override; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..d996555365166 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" +#include "behavior_path_lane_change_module/manager.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ +using route_handler::Direction; + +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager() + : LaneChangeModuleManager( + "avoidance_by_lc", route_handler::Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) + { + } + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override; + +private: + std::shared_ptr avoidance_parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp similarity index 77% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp rename to planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 67fd2c1ba0c77..7a82ba60a1eaa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; using AvoidanceDebugData = DebugData; +using helper::avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { @@ -48,6 +48,7 @@ class AvoidanceByLaneChange : public NormalLaneChange ObjectDataArray registered_objects_; mutable ObjectDataArray stopped_objects_; + std::shared_ptr avoidance_helper_; ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; @@ -56,4 +57,4 @@ class AvoidanceByLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml new file mode 100644 index 0000000000000..3a4f42f2d2a6c --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -0,0 +1,40 @@ + + + + behavior_path_avoidance_by_lane_change_module + 0.1.0 + The behavior_path_avoidance_by_lane_change_module package + + Satoshi Ota + Zulfaqar Azmi + Fumiya Watanabe + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_avoidance_module + behavior_path_lane_change_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..092d54c096ae9 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp new file mode 100644 index 0000000000000..efdb302a58ac2 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -0,0 +1,66 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" + +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" + +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: LaneChangeInterface{ + name, + node, + parameters, + rtc_interface_ptr_map, + objects_of_interest_marker_interface_ptr_map, + std::make_unique(parameters, avoidance_by_lane_change_parameters)} +{ +} + +bool AvoidanceByLaneChangeInterface::isExecutionRequested() const +{ + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck(); +} +void AvoidanceByLaneChangeInterface::processOnEntry() +{ + waitApproval(); +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..3f810710ef37b --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -0,0 +1,192 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_avoidance_by_lane_change_module/manager.hpp" + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ + +void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; + + // init manager interface + initInterface(node, {"left", "right"}); + + // init lane change manager + LaneChangeModuleManager::init(node); + + const auto avoidance_params = getParameter(node); + AvoidanceByLCParameters p(avoidance_params); + + // unique parameters + { + const std::string ns = "avoidance_by_lane_change."; + p.execute_object_longitudinal_margin = + getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = + getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); + } + + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + return param; + }; + + const std::string ns = "avoidance_by_lane_change.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + const std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + + // safety check + { + const std::string ns = "avoidance.safety_check."; + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + } + + avoidance_parameters_ = std::make_shared(p); +} + +std::unique_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::AvoidanceByLaneChangeModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp similarity index 65% rename from planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp rename to planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index befd458153e6e..619cbc515f816 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" +#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" -#include -#include +#include #include #include @@ -28,11 +29,22 @@ namespace behavior_path_planner { +namespace +{ +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} +} // namespace AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), - avoidance_parameters_(std::move(avoidance_parameters)) + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_helper_{std::make_shared(avoidance_parameters_)} { } @@ -40,48 +52,66 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const { const auto & data = avoidance_data_; - if (!status_.is_safe) { + if (data.target_objects.empty()) { return false; } const auto & object_parameters = avoidance_parameters_->object_parameters; - const auto is_more_than_threshold = - std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) { - const auto & objects = avoidance_data_.target_objects; - - const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) { - const auto target_class = - utils::getHighestProbLabel(object.object.classification) == p.first; - return target_class && object.avoid_required; - }); - return num >= p.second.execute_num; - }); + const auto count_target_object = [&](const auto sum, const auto & p) { + const auto & objects = avoidance_data_.target_objects; - if (!is_more_than_threshold) { - return false; - } + const auto is_avoidance_target = [&p](const auto & object) { + const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }; - const auto & front_object = data.target_objects.front(); + return sum + std::count_if(objects.begin(), objects.end(), is_avoidance_target); + }; + const auto num_of_avoidance_targets = + std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object); - const auto THRESHOLD = avoidance_parameters_->execute_object_longitudinal_margin; - if (front_object.longitudinal < THRESHOLD) { + if (num_of_avoidance_targets < 1) { return false; } - const auto path = status_.lane_change_path.path; - const auto to_lc_end = motion_utils::calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.info.shift_line.end.position); - const auto execute_only_when_lane_change_finish_before_object = - avoidance_parameters_->execute_only_when_lane_change_finish_before_object; - const auto not_enough_distance = front_object.longitudinal < to_lc_end; - - if (execute_only_when_lane_change_finish_before_object && not_enough_distance) { + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { return false; } - return true; + const auto & nearest_object = data.target_objects.front(); + + // get minimum lane change distance + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, + lane_change_parameters_->backward_length_buffer_for_end_of_lane); + + // get minimum avoid distance + + const auto ego_width = getCommonParam().vehicle_width; + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); + const auto nearest_object_parameter = + avoidance_parameters_->object_parameters.at(nearest_object_type); + const auto avoid_margin = + nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + + nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + + const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); + + RCLCPP_DEBUG( + logger_, + "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " + "%.3f", + nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); + + return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); } bool AvoidanceByLaneChange::specialExpiredCheck() const @@ -128,6 +158,23 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( data.current_lanelets = getCurrentLanes(); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, avoidance_parameters_)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, + avoidance_parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, + avoidance_parameters_->use_intersection_areas, false)); + // get related objects from dynamic_objects, and then separates them as target objects and non // target objects fillAvoidanceTargetObjects(data, debug); diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..3041ae7e1112e --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,130 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + + "/config/avoidance.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + + "/config/avoidance_by_lc.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_avoidance_module/CMakeLists.txt b/planning/behavior_path_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..c94591f9bbd30 --- /dev/null +++ b/planning/behavior_path_avoidance_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/debug.cpp + src/utils.cpp + src/manager.cpp + src/shift_line_generator.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_utils.cpp + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/avoidance/avoidance.param.yaml rename to planning/behavior_path_avoidance_module/config/avoidance.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp similarity index 98% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 68c64ff9b7112..8e294e9e3237e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -401,6 +401,9 @@ struct ObjectData // avoidance target // lateral avoid margin std::optional avoid_margin{std::nullopt}; + + // the nearest bound point (use in road shoulder distance calculation) + std::optional nearest_bound_point{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -517,6 +520,8 @@ struct AvoidancePlanningData bool valid{false}; + bool success{false}; + bool comfortable{false}; bool avoid_required{false}; @@ -622,4 +627,4 @@ struct DebugData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 12b54a5031041..2705af5af4020 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include @@ -59,13 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray makeOverhangToRoadShoulderMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); - -MarkerArray createOverhangFurthestLineStringMarkerArray( - const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, - const float & g, const float & b); - MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker @@ -78,4 +71,4 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 54a38b828e987..df9640fa75626 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include @@ -58,6 +58,8 @@ class AvoidanceHelper double getEgoSpeed() const { return std::abs(data_->self_odometry->twist.twist.linear.x); } + geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } + size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const { const auto itr = std::find_if( @@ -185,10 +187,20 @@ class AvoidanceHelper return parameters_->object_check_max_forward_distance; } + const auto & route_handler = data_->route_handler; + + lanelet::ConstLanelet closest_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &closest_lane)) { + return parameters_->object_check_max_forward_distance; + } + + const auto limit = route_handler->getTrafficRulesPtr()->speedLimit(closest_lane); + const auto speed = isShifted() ? limit.speedLimit.value() : getEgoSpeed(); + const auto max_shift_length = std::max( std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); - const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( - max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + const auto dynamic_distance = + PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed); return std::clamp( 1.5 * dynamic_distance + getNominalPrepareDistance(), @@ -331,4 +343,4 @@ class AvoidanceHelper }; } // namespace behavior_path_planner::helper::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 0fe8bd7efa17b..72cfbe6f0a1ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -51,4 +51,4 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp similarity index 73% rename from planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index e17804f10deaa..212ed04ade6c6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -11,28 +11,26 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. - -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ #include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include +#include +#include + +#include -#include #include #include namespace behavior_path_planner { -void AvoidanceModuleManager::init(rclcpp::Node * node) -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::getOrDeclareParameter; - - // init manager interface - initInterface(node, {"left", "right"}); +using autoware_auto_perception_msgs::msg::ObjectClassification; +using tier4_autoware_utils::getOrDeclareParameter; +AvoidanceParameters getParameter(rclcpp::Node * node) +{ AvoidanceParameters p{}; // general params { @@ -359,134 +357,8 @@ void AvoidanceModuleManager::init(rclcpp::Node * node) p.sharp_shift_filter_threshold = getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); } - - parameters_ = std::make_shared(p); -} - -void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::updateParam; - - auto p = parameters_; - - { - const std::string ns = "avoidance."; - updateParam(parameters, ns + "enable_safety_check", p->enable_safety_check); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - updateParam(parameters, ns + "print_debug_info", p->print_debug_info); - } - - const auto update_object_param = [&p, ¶meters]( - const auto & semantic, const std::string & ns) { - auto & config = p->object_parameters.at(semantic); - updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); - updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); - updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); - updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); - updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); - updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); - updateParam( - parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); - updateParam( - parameters, ns + "use_conservative_buffer_longitudinal", - config.use_conservative_buffer_longitudinal); - }; - - { - const std::string ns = "avoidance.target_object."; - update_object_param(ObjectClassification::MOTORCYCLE, ns + "motorcycle."); - update_object_param(ObjectClassification::CAR, ns + "car."); - update_object_param(ObjectClassification::TRUCK, ns + "truck."); - update_object_param(ObjectClassification::TRAILER, ns + "trailer."); - update_object_param(ObjectClassification::BUS, ns + "bus."); - update_object_param(ObjectClassification::PEDESTRIAN, ns + "pedestrian."); - update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); - update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); - - updateParam( - parameters, ns + "lower_distance_for_polygon_expansion", - p->lower_distance_for_polygon_expansion); - updateParam( - parameters, ns + "upper_distance_for_polygon_expansion", - p->upper_distance_for_polygon_expansion); - } - - { - const std::string ns = "avoidance.avoidance.lateral."; - updateParam( - parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold); - updateParam( - parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); - updateParam( - parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold); - updateParam(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin); - updateParam(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin); - } - - { - const std::string ns = "avoidance.avoidance.longitudinal."; - updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); - updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); - updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); - updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); - } - - { - const std::string ns = "avoidance.stop."; - updateParam(parameters, ns + "max_distance", p->stop_max_distance); - updateParam(parameters, ns + "stop_buffer", p->stop_buffer); - } - - { - const std::string ns = "avoidance.constrains.lateral."; - - std::vector velocity_map; - updateParam>(parameters, ns + "velocity", velocity_map); - std::vector lateral_max_accel_map; - updateParam>(parameters, ns + "max_accel_values", lateral_max_accel_map); - std::vector lateral_min_jerk_map; - updateParam>(parameters, ns + "min_jerk_values", lateral_min_jerk_map); - std::vector lateral_max_jerk_map; - updateParam>(parameters, ns + "max_jerk_values", lateral_max_jerk_map); - - if (!velocity_map.empty()) { - p->velocity_map = velocity_map; - } - - if (!velocity_map.empty() && velocity_map.size() == lateral_max_accel_map.size()) { - p->lateral_max_accel_map = lateral_max_accel_map; - } - - if (!velocity_map.empty() && velocity_map.size() == lateral_min_jerk_map.size()) { - p->lateral_min_jerk_map = lateral_min_jerk_map; - } - - if (!velocity_map.empty() && velocity_map.size() == lateral_max_jerk_map.size()) { - p->lateral_max_jerk_map = lateral_max_jerk_map; - } - } - - { - const std::string ns = "avoidance.shift_line_pipeline."; - updateParam( - parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); - updateParam( - parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); - } - - std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { - if (!observer.expired()) observer.lock()->updateModuleParams(p); - }); + return p; } } // namespace behavior_path_planner -#include -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index b01cb62f22372..882806177b4e0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/helper.hpp" -#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -73,6 +73,8 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr get_debug_msg_array() const; private: + bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const; + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } @@ -450,4 +452,4 @@ class AvoidanceModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index 054a6e15ef6bf..edfe9ab9663ce 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -248,4 +248,4 @@ class ShiftLineGenerator } // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index e7611208962f0..521058e3a43c9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -135,13 +135,6 @@ double getRoadShoulderDistance( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -double extendToRoadShoulderDistanceWithPolygon( - const std::shared_ptr & rh, - const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, - const bool use_intersection_areas); - void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); @@ -180,4 +173,4 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml new file mode 100644 index 0000000000000..642f83497acfb --- /dev/null +++ b/planning/behavior_path_avoidance_module/package.xml @@ -0,0 +1,56 @@ + + + + behavior_path_avoidance_module + 0.1.0 + The behavior_path_avoidance_module package + + Takamasa Horibe + Satoshi Ota + Fumiya Watanabe + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + autoware_perception_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + magic_enum + motion_utils + objects_of_interest_marker_interface + pluginlib + rclcpp + route_handler + rtc_interface + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_avoidance_module/plugins.xml b/planning/behavior_path_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..f25677dad1e9a --- /dev/null +++ b/planning/behavior_path_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp similarity index 90% rename from planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp rename to planning/behavior_path_avoidance_module/src/debug.cpp index 7587c3bb3663e..310613ee19678 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" +#include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -109,6 +109,42 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: return msg; } +MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + for (const auto & object : objects) { + if (!object.nearest_bound_point.has_value()) { + continue; + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999)); + + marker.points.push_back(object.overhang_pose.position); + marker.points.push_back(object.nearest_bound_point.value()); + marker.id = uuidToInt32(object.object.object_id); + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.pose.position = object.nearest_bound_point.value(); + std::ostringstream string_stream; + string_stream << object.to_road_shoulder_distance << "[m]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + } + + return msg; +} + MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; @@ -166,6 +202,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); return msg; } @@ -183,6 +220,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); return msg; } @@ -418,58 +456,37 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray makeOverhangToRoadShoulderMarkerArray( - const ObjectDataArray & objects, std::string && ns) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0)); - - int32_t i{0}; - MarkerArray msg; - for (const auto & object : objects) { - marker.id = ++i; - marker.pose = object.overhang_pose; - std::ostringstream string_stream; - string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])"; - marker.text = string_stream.str(); - msg.markers.push_back(marker); - } - - return msg; -} - -MarkerArray createOverhangFurthestLineStringMarkerArray( - const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, - const float & g, const float & b) +MarkerArray createDrivableBounds( + const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, + const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); MarkerArray msg; - for (const auto & linestring : linestrings) { - const auto id = static_cast(linestring.id()); + // right bound + { auto marker = createDefaultMarker( - "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), + "map", current_time, ns + "_right", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - for (const auto & p : linestring.basicLineString()) { + for (const auto & p : data.right_bound) { marker.points.push_back(createPoint(p.x(), p.y(), p.z())); } + msg.markers.push_back(marker); + } + + // left bound + { + auto marker = createDefaultMarker( + "map", current_time, ns + "_left", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), + createMarkerColor(r, g, b, 0.999)); + + for (const auto & p : data.left_bound) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } - Marker marker_linestring_id = createDefaultMarker( - "map", current_time, "linestring_id", id, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.8)); - Pose text_id_pose; - text_id_pose.position.x = linestring.front().x(); - text_id_pose.position.y = linestring.front().y(); - text_id_pose.position.z = linestring.front().z(); - marker_linestring_id.pose = text_id_pose; - std::ostringstream ss; - ss << "(ID : " << id << ") "; - marker_linestring_id.text = ss.str(); - msg.markers.push_back(marker_linestring_id); + msg.markers.push_back(marker); } return msg; @@ -593,8 +610,7 @@ MarkerArray createDebugMarkerArray( add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); } return msg; diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp new file mode 100644 index 0000000000000..68cee1aa2b523 --- /dev/null +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -0,0 +1,168 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_avoidance_module/manager.hpp" + +#include "behavior_path_avoidance_module/parameter_helper.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ +void AvoidanceModuleManager::init(rclcpp::Node * node) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; + + // init manager interface + initInterface(node, {"left", "right"}); + + auto p = getParameter(node); + + parameters_ = std::make_shared(p); +} + +void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::updateParam; + + auto p = parameters_; + + { + const std::string ns = "avoidance."; + updateParam(parameters, ns + "enable_safety_check", p->enable_safety_check); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + } + + const auto update_object_param = [&p, ¶meters]( + const auto & semantic, const std::string & ns) { + auto & config = p->object_parameters.at(semantic); + updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); + updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); + updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); + updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); + updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); + updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); + updateParam( + parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + updateParam( + parameters, ns + "use_conservative_buffer_longitudinal", + config.use_conservative_buffer_longitudinal); + }; + + { + const std::string ns = "avoidance.target_object."; + update_object_param(ObjectClassification::MOTORCYCLE, ns + "motorcycle."); + update_object_param(ObjectClassification::CAR, ns + "car."); + update_object_param(ObjectClassification::TRUCK, ns + "truck."); + update_object_param(ObjectClassification::TRAILER, ns + "trailer."); + update_object_param(ObjectClassification::BUS, ns + "bus."); + update_object_param(ObjectClassification::PEDESTRIAN, ns + "pedestrian."); + update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); + update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); + + updateParam( + parameters, ns + "lower_distance_for_polygon_expansion", + p->lower_distance_for_polygon_expansion); + updateParam( + parameters, ns + "upper_distance_for_polygon_expansion", + p->upper_distance_for_polygon_expansion); + } + + { + const std::string ns = "avoidance.avoidance.lateral."; + updateParam( + parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold); + updateParam( + parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); + updateParam( + parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold); + updateParam(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin); + updateParam(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin); + } + + { + const std::string ns = "avoidance.avoidance.longitudinal."; + updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); + updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); + updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); + updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); + } + + { + const std::string ns = "avoidance.stop."; + updateParam(parameters, ns + "max_distance", p->stop_max_distance); + updateParam(parameters, ns + "stop_buffer", p->stop_buffer); + } + + { + const std::string ns = "avoidance.constrains.lateral."; + + std::vector velocity_map; + updateParam>(parameters, ns + "velocity", velocity_map); + std::vector lateral_max_accel_map; + updateParam>(parameters, ns + "max_accel_values", lateral_max_accel_map); + std::vector lateral_min_jerk_map; + updateParam>(parameters, ns + "min_jerk_values", lateral_min_jerk_map); + std::vector lateral_max_jerk_map; + updateParam>(parameters, ns + "max_jerk_values", lateral_max_jerk_map); + + if (!velocity_map.empty()) { + p->velocity_map = velocity_map; + } + + if (!velocity_map.empty() && velocity_map.size() == lateral_max_accel_map.size()) { + p->lateral_max_accel_map = lateral_max_accel_map; + } + + if (!velocity_map.empty() && velocity_map.size() == lateral_min_jerk_map.size()) { + p->lateral_min_jerk_map = lateral_min_jerk_map; + } + + if (!velocity_map.empty() && velocity_map.size() == lateral_max_jerk_map.size()) { + p->lateral_max_jerk_map = lateral_max_jerk_map; + } + } + + { + const std::string ns = "avoidance.shift_line_pipeline."; + updateParam( + parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); + updateParam( + parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); + } + + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); + }); +} +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::AvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp similarity index 96% rename from planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp rename to planning/behavior_path_avoidance_module/src/scene.cpp index df93df75410e1..ebe42f73d538b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_avoidance_module/scene.hpp" -#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -137,7 +137,48 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoid_data_.safe && avoid_data_.comfortable; + return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid; +} + +bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const +{ + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); + + if (has_avoidance_target) { + return false; + } + + // If the ego is on the shift line, keep RUNNING. + { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; + }; + for (const auto & shift_line : path_shifter_.getShiftLines()) { + if (within(shift_line, idx)) { + return false; + } + } + } + + const bool has_shift_point = !path_shifter_.getShiftLines().empty(); + const bool has_base_offset = + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + + // Nothing to do. -> EXIT. + if (!has_shift_point && !has_base_offset) { + return true; + } + + // Be able to canceling avoidance path. -> EXIT. + if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { + return true; + } + + return false; } bool AvoidanceModule::canTransitSuccessState() @@ -168,44 +209,7 @@ bool AvoidanceModule::canTransitSuccessState() } } - const bool has_avoidance_target = - std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { - return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; - }); - const bool has_shift_point = !path_shifter_.getShiftLines().empty(); - const bool has_base_offset = - std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; - - // If the vehicle is on the shift line, keep RUNNING. - { - const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto within = [](const auto & line, const size_t idx) { - return line.start_idx < idx && idx < line.end_idx; - }; - for (const auto & shift_line : path_shifter_.getShiftLines()) { - if (within(shift_line, idx)) { - return false; - } - } - } - - // Nothing to do. -> EXIT. - if (!has_avoidance_target) { - if (!has_shift_point && !has_base_offset) { - RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); - return true; - } - } - - // Be able to canceling avoidance path. -> EXIT. - if (!has_avoidance_target) { - if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { - RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); - return true; - } - } - - return false; // Keep current state. + return data.success; } void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) @@ -216,7 +220,7 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // lanelet info data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_); + getPreviousModuleOutput().reference_path, planner_data_); data.extend_lanelets = utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); @@ -229,8 +233,8 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat }); // calc drivable bound - const auto shorten_lanes = - utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + auto tmp_path = getPreviousModuleOutput().path; + const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, true)); @@ -240,9 +244,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { - data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + data.reference_path_rough = extendBackwardLength(getPreviousModuleOutput().path); } else { - data.reference_path_rough = *getPreviousModuleOutput().path; + data.reference_path_rough = getPreviousModuleOutput().path; RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path."); } @@ -450,15 +454,14 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP1: Create candidate shift lines. * Merge rough shift lines, and extract new shift lines. */ - const auto processed_shift_lines = generator_.generate(data, debug); + data.new_shift_line = generator_.generate(data, debug); /** * Step2: Validate new shift lines. * Output new shift lines only when the avoidance path which is generated from them doesn't have * huge offset from ego. */ - data.valid = isValidShiftLine(processed_shift_lines, path_shifter); - data.new_shift_line = data.valid ? processed_shift_lines : AvoidLineArray{}; + data.valid = isValidShiftLine(data.new_shift_line, path_shifter); const auto found_new_sl = data.new_shift_line.size() > 0; const auto registered = path_shifter.getShiftLines().size() > 0; data.found_avoidance_path = found_new_sl || registered; @@ -494,16 +497,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - /** - * TODO(someone): prevent meaningless stop point insertion in other way. - * If the candidate shift line is invalid, manage all objects as unavoidable. - */ - if (!data.valid) { - std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { - o.is_avoidable = false; - o.reason = "InvalidShiftLine"; - }); - } + data.success = isSatisfiedSuccessCondition(data); /** * Find the nearest object that should be avoid. When the ego follows reference path, @@ -857,6 +851,10 @@ BehaviorModuleOutput AvoidanceModule::plan() updatePathShifter(data.safe_shift_line); + if (data.success) { + removeRegisteredShiftLines(); + } + if (data.yield_required) { removeRegisteredShiftLines(); } @@ -912,17 +910,17 @@ BehaviorModuleOutput AvoidanceModule::plan() } if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { - output.path = std::make_shared(spline_shift_path.path); + output.path = spline_shift_path.path; } else { output.path = getPreviousModuleOutput().path; RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing."); } output.reference_path = getPreviousModuleOutput().reference_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - const size_t ego_idx = planner_data_->findEgoIndex(output.path->points); - utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); + const size_t ego_idx = planner_data_->findEgoIndex(output.path.points); + utils::clipPathLength(output.path, ego_idx, planner_data_->parameters); // Drivable area generation. { @@ -997,7 +995,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() } path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return out; } @@ -1171,11 +1169,11 @@ void AvoidanceModule::updateData() helper_->setData(planner_data_); if (!helper_->isInitialized()) { - helper_->setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_->setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_->setPreviousReferencePath(*getPreviousModuleOutput().path); + helper_->setPreviousSplineShiftPath(toShiftedPath(getPreviousModuleOutput().path)); + helper_->setPreviousLinearShiftPath(toShiftedPath(getPreviousModuleOutput().path)); + helper_->setPreviousReferencePath(getPreviousModuleOutput().path); helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_)); + getPreviousModuleOutput().reference_path, planner_data_)); } debug_data_ = DebugData(); @@ -1702,7 +1700,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); - if (current_target_velocity < getEgoSpeed() && decel_distance < remaining_distance) { + if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::avoidance::insertDecelPoint( getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, slow_pose_); diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp rename to planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index c7fbaa931737c..4332008b95e70 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/shift_line_generator.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -931,6 +931,9 @@ void ShiftLineGenerator::applySimilarGradFilter( combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx, input.at(i).end_longitudinal); + combine.parent_ids = + utils::avoidance::concatParentIds(base_line.parent_ids, input.at(i).parent_ids); + combine_buffer.push_back(input.at(i)); const auto violates = [&]() { @@ -966,6 +969,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( AvoidLineArray ret = shift_lines; constexpr double ep = 1.0e-3; + constexpr double RETURN_SHIFT_THRESHOLD = 0.1; const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); @@ -977,14 +981,15 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - // If the return-to-center shift points are already registered, do nothing. - if (!has_registered_point && std::fabs(base_offset_) < ep) { - return ret; - } - - constexpr double RETURN_SHIFT_THRESHOLD = 0.1; - if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) { - return ret; + if (last_.has_value()) { + if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) { + return ret; + } + } else { + // If the return-to-center shift points are already registered, do nothing. + if (std::abs(base_offset_) < ep) { + return ret; + } } // From here, the return-to-center is not registered. But perhaps the candidate is diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp similarity index 91% rename from planning/behavior_path_planner/src/utils/avoidance/utils.cpp rename to planning/behavior_path_avoidance_module/src/utils.cpp index 6fd1943369cb9..fd120bdd8a069 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,8 +14,8 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" @@ -756,11 +756,21 @@ size_t findPathIndexFromArclength( std::vector concatParentIds(const std::vector & ids1, const std::vector & ids2) { std::vector ret; - for (const auto id2 : ids2) { - if (std::any_of(ids1.begin(), ids1.end(), [&id2](const auto & id1) { return id1 == id2; })) { + + for (const auto & id : ids1) { + if (std::any_of( + ret.begin(), ret.end(), [&id](const auto & exist_id) { return exist_id == id; })) { + continue; + } + ret.push_back(id); + } + + for (const auto & id : ids2) { + if (std::any_of( + ret.begin(), ret.end(), [&id](const auto & exist_id) { return exist_id == id; })) { continue; } - ret.push_back(id2); + ret.push_back(id); } return ret; @@ -1348,76 +1358,60 @@ void compensateDetectionLost( double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + [[maybe_unused]] const std::shared_ptr & parameters) { using lanelet::utils::to2D; + using tier4_autoware_utils::Point2d; const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - findNearestIndex(data.reference_path_rough.points, object_pose.position); - const auto object_closest_pose = - data.reference_path_rough.points.at(object_closest_index).point.pose; + findNearestIndex(data.reference_path.points, object_pose.position); + const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { return 0.0; } - double road_shoulder_distance = std::numeric_limits::max(); - - const bool get_left = isOnRight(object) && parameters->use_adjacent_lane; - const bool get_right = !isOnRight(object) && parameters->use_adjacent_lane; - const bool get_opposite = parameters->use_opposite_lane; - - lanelet::BasicPoint3d p_overhang( - object.overhang_pose.position.x, object.overhang_pose.position.y, - object.overhang_pose.position.z); - - lanelet::ConstLineString3d target_line{}; - - const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { - const auto lines = - rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); - const auto & line = isOnRight(object) ? lines.back() : lines.front(); - const auto d = boost::geometry::distance(object.envelope_poly, to2D(line.basicLineString())); - if (d < road_shoulder_distance) { - road_shoulder_distance = d; - target_line = line; + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto & p1_object = object.overhang_pose.position; + const auto p_tmp = + geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); + const auto p2_object = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + std::vector intersects; + for (size_t i = 1; i < bound.size(); i++) { + const auto p1_bound = + geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); + const auto p2_bound = + geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); + + const auto opt_intersect = + tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); + + if (!opt_intersect) { + continue; } - }; - - // current lanelet - { - update_road_to_shoulder_distance(object.overhang_lanelet); - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, object.overhang_lanelet, - object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); + intersects.push_back(opt_intersect.value()); } - // previous lanelet - lanelet::ConstLanelets previous_lanelet{}; - if (rh->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &previous_lanelet)) { - update_road_to_shoulder_distance(previous_lanelet.front()); - - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, previous_lanelet.front(), - object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); + if (intersects.empty()) { + return 0.0; } - // next lanelet - lanelet::ConstLanelet next_lanelet{}; - if (rh->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { - update_road_to_shoulder_distance(next_lanelet); + std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { + return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); + }); - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, next_lanelet, object.overhang_pose.position, - p_overhang, parameters->use_hatched_road_markings, parameters->use_intersection_areas); - } + object.nearest_bound_point = intersects.front(); - return road_shoulder_distance; + return calcDistance2d(p1_object, object.nearest_bound_point.value()); } void filterTargetObjects( @@ -1465,93 +1459,6 @@ void filterTargetObjects( } } -double extendToRoadShoulderDistanceWithPolygon( - const std::shared_ptr & rh, - const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, - const bool use_intersection_areas) -{ - // get expandable polygons for avoidance (e.g. hatched road markings) - std::vector expandable_polygons; - - const auto exist_polygon = [&](const auto & candidate_polygon) { - return std::any_of( - expandable_polygons.begin(), expandable_polygons.end(), - [&](const auto & polygon) { return polygon.id() == candidate_polygon.id(); }); - }; - - if (use_hatched_road_markings) { - for (const auto & point : target_line) { - const auto new_polygon_candidate = - utils::getPolygonByPoint(rh, point, "hatched_road_markings"); - - if (!!new_polygon_candidate && !exist_polygon(*new_polygon_candidate)) { - expandable_polygons.push_back(*new_polygon_candidate); - } - } - } - - if (use_intersection_areas) { - const std::string area_id_str = overhang_lanelet.attributeOr("intersection_area", "else"); - - if (area_id_str != "else") { - expandable_polygons.push_back( - rh->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id_str.c_str()))); - } - } - - if (expandable_polygons.empty()) { - return to_road_shoulder_distance; - } - - // calculate point laterally offset from overhang position to calculate intersection with - // polygon - Point lat_offset_overhang_pos; - { - auto arc_coordinates = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(target_line), lanelet::utils::to2D(overhang_basic_pose)); - arc_coordinates.distance = 0.0; - const auto closest_target_line_point = - lanelet::geometry::fromArcCoordinates(target_line, arc_coordinates); - - const double ratio = 100.0 / to_road_shoulder_distance; - lat_offset_overhang_pos.x = - closest_target_line_point.x() + (closest_target_line_point.x() - overhang_pos.x) * ratio; - lat_offset_overhang_pos.y = - closest_target_line_point.y() + (closest_target_line_point.y() - overhang_pos.y) * ratio; - } - - // update to_road_shoulder_distance with valid expandable polygon - double updated_to_road_shoulder_distance = to_road_shoulder_distance; - for (const auto & polygon : expandable_polygons) { - std::vector intersect_dist_vec; - for (size_t i = 0; i < polygon.size(); ++i) { - const auto polygon_current_point = - geometry_msgs::build().x(polygon[i].x()).y(polygon[i].y()).z(0.0); - const auto polygon_next_point = geometry_msgs::build() - .x(polygon[(i + 1) % polygon.size()].x()) - .y(polygon[(i + 1) % polygon.size()].y()) - .z(0.0); - - const auto intersect_pos = tier4_autoware_utils::intersect( - overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point); - if (intersect_pos) { - intersect_dist_vec.push_back(calcDistance2d(*intersect_pos, overhang_pos)); - } - } - - if (intersect_dist_vec.empty()) { - continue; - } - - std::sort(intersect_dist_vec.begin(), intersect_dist_vec.end()); - updated_to_road_shoulder_distance = - std::max(updated_to_road_shoulder_distance, intersect_dist_vec.back()); - } - return updated_to_road_shoulder_distance; -} - AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line) { AvoidLineArray ret{line}; diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..58bf36d978cea --- /dev/null +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,124 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + + "/config/avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_avoidance_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp similarity index 92% rename from planning/behavior_path_planner/test/test_avoidance_utils.cpp rename to planning/behavior_path_avoidance_module/test/test_utils.cpp index fc2bea1a5b288..e5c6fd2e72092 100644 --- a/planning/behavior_path_planner/test/test_avoidance_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -11,8 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" + +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include #include diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..86dc0ccb61330 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_external_request_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/scene.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package() diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..dfcc4f61d8241 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_lane_change_module/manager.hpp" +#include "route_handler/route_handler.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeRightModuleManager() + : LaneChangeModuleManager( + "external_request_lane_change_right", route_handler::Direction::RIGHT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; + +class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeLeftModuleManager() + + : LaneChangeModuleManager( + "external_request_lane_change_left", route_handler::Direction::LEFT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp similarity index 79% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp rename to planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp index d9c77d6db337e..c23d6f2f3996c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include @@ -35,4 +35,4 @@ class ExternalRequestLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_external_request_lane_change_module/package.xml new file mode 100644 index 0000000000000..16216aa1f71e9 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/package.xml @@ -0,0 +1,39 @@ + + + + behavior_path_external_request_lane_change_module + 0.1.0 + The behavior_path_external_request_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_lane_change_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..f3cc686837c38 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..3cfe7aa51f0f1 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_external_request_lane_change_module/manager.hpp" + +#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/interface.hpp" + +namespace behavior_path_planner +{ + +std::unique_ptr +ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +std::unique_ptr +ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 92% rename from planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp rename to planning/behavior_path_external_request_lane_change_module/src/scene.cpp index 3a304a9b5bb53..913266bf79fa3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" +#include "behavior_path_external_request_lane_change_module/scene.hpp" #include diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..ff5be388704da --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,128 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, { + planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + }); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_goal_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..c91289bf29e2c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_goal_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/default_fixed_goal_planner.cpp + src/freespace_pull_over.cpp + src/geometric_pull_over.cpp + src/goal_searcher.cpp + src/shift_pull_over.cpp + src/util.cpp + src/goal_planner_module.cpp + src/manager.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml rename to planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 1595503a225f8..4da625a7a9087 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#include "behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp" +#include "behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include @@ -42,4 +42,4 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 5fe3427749844..0926d1010b5c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" @@ -50,4 +50,4 @@ class FixedGoalPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index bb5d7b7555cfc..5ce6d83795069 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" +#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" #include #include @@ -51,4 +51,4 @@ class FreespacePullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index d96b01582a2d1..6de1a726d4d4e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include @@ -69,4 +69,4 @@ class GeometricPullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index a9bc31983ac6c..e90162c958be5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ - -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" -#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" -#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" -#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ + +#include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "behavior_path_goal_planner_module/goal_searcher.hpp" +#include "behavior_path_goal_planner_module/shift_pull_over.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -282,6 +282,19 @@ class GoalPlannerModule : public SceneModuleInterface bool isExecutionReady() const override; void processOnExit() override; void updateData() override; + + void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params); + + void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params); + + void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params); + void postProcess() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( @@ -291,7 +304,7 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - /*　 + /* * state transitions and plan function used in each state * * +--------------------------+ @@ -463,10 +476,6 @@ class GoalPlannerModule : public SceneModuleInterface void onTimer(); void onFreespaceParkingTimer(); - // flag for the interface which do not support `allow_goal_modification` - // when the goal is in `road_shoulder`, always allow goal modification. - bool checkOriginalGoalIsInShoulder() const; - // steering factor void updateSteeringFactor( const std::array & pose, const std::array distance, const uint16_t type); @@ -494,4 +503,4 @@ class GoalPlannerModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index c5adf3a2ad652..21eb22046bff6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -124,4 +124,4 @@ struct GoalPlannerParameters }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 2fc0acf1c5086..94cf7bc7ff5dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include @@ -55,4 +55,4 @@ class GoalSearcher : public GoalSearcherBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index 7d1c99a69b1d5..93472ab6c0703 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include @@ -69,4 +69,4 @@ class GoalSearcherBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp index a166a677c7388..aefd4cac3ee35 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" +#include "behavior_path_goal_planner_module/goal_planner_module.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -55,4 +55,4 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index a0bfd8e883d80..ddd7c93998654 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" @@ -135,4 +135,4 @@ class PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index 9247ec566fbd8..bd19dc2e87de0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -61,4 +61,4 @@ class ShiftPullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 25c0551d896b3..791c35f3afca6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" +#include "behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -71,4 +71,4 @@ MarkerArray createNumObjectsToAvoidTextsMarkerArray( } // namespace goal_planner_utils } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml new file mode 100644 index 0000000000000..213c0093b08d9 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_goal_planner_module + 0.1.0 + The behavior_path_goal_planner_module package + + Kosuke Takeuchi + Kyoichi Sugahara + Satoshi OTA + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_goal_planner_module/plugins.xml b/planning/behavior_path_goal_planner_module/plugins.xml new file mode 100644 index 0000000000000..6c3b557e35e7a --- /dev/null +++ b/planning/behavior_path_goal_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp similarity index 92% rename from planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp rename to planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 9ae50def1d358..cbbe5f585dbe2 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" +#include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -33,9 +33,8 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( BehaviorModuleOutput output = // use planner previous module reference path getPreviousModuleOutput(); - const PathWithLaneId smoothed_path = - modifyPathForSmoothGoalConnection(*(output.path), planner_data); - output.path = std::make_shared(smoothed_path); + const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(output.path, planner_data); + output.path = smoothed_path; output.reference_path = getPreviousModuleOutput().reference_path; return output; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp similarity index 94% rename from planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp rename to planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index d5252028f880b..c9af9ee369bd5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" +#include "behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include @@ -116,7 +116,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) std::vector> pairs_terminal_velocity_and_accel{}; pairs_terminal_velocity_and_accel.resize(partial_paths.size()); - utils::start_goal_planner_common::modifyVelocityByDirection( + utils::parking_departure::modifyVelocityByDirection( partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); // Check if driving forward for each path, return empty if not diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp similarity index 95% rename from planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp rename to planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 55d8bc6cfda3f..31c1d3ef77b0e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" +#include "behavior_path_goal_planner_module/geometric_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include #include diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp similarity index 96% rename from planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp rename to planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index e8905f64e9617..2ea6f1e100e91 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" +#include "behavior_path_goal_planner_module/goal_planner_module.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -39,7 +39,7 @@ #include #include -using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; +using behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -120,8 +120,7 @@ GoalPlannerModule::GoalPlannerModule( // Initialize safety checker if (parameters_->safety_check_params.enable_safety_check) { initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); + utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); } prev_data_.reset(); @@ -242,6 +241,30 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } +void GoalPlannerModule::updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void GoalPlannerModule::updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void GoalPlannerModule::updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + void GoalPlannerModule::updateData() { if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { @@ -259,7 +282,7 @@ void GoalPlannerModule::updateData() resetPathCandidate(); resetPathReference(); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); updateOccupancyGrid(); @@ -303,11 +326,9 @@ void GoalPlannerModule::initializeOccupancyGridMap() void GoalPlannerModule::initializeSafetyCheckParameters() { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); + updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); + updateSafetyCheckParams(safety_check_params_, parameters_); + updateObjectsFilteringParams(objects_filtering_params_, parameters_); } void GoalPlannerModule::processOnExit() @@ -740,7 +761,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const // because it takes time for the trajectory to be reflected auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); keepStoppedWithCurrentPath(current_path); - output.path = std::make_shared(current_path); + output.path = current_path; } setModifiedGoal(output); @@ -756,14 +777,14 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const { if (prev_data_.found_path || !prev_data_.stop_path) { // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = std::make_shared(generateStopPath()); + output.path = generateStopPath(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = prev_data_.stop_path; + output.path = *prev_data_.stop_path; // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); + stop_pose_ = utils::getFirstStopPoseFromPath(output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } @@ -775,24 +796,23 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = - behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); if (stop_path) { - output.path = std::make_shared(*stop_path); + output.path = *stop_path; RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { - output.path = - std::make_shared(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = prev_data_.stop_path_after_approval; + output.path = *prev_data_.stop_path_after_approval; // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); + stop_pose_ = utils::getFirstStopPoseFromPath(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } } @@ -805,7 +825,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); + output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -832,9 +852,9 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const { const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( - *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); } @@ -852,7 +872,6 @@ void GoalPlannerModule::updateSteeringFactor( return SteeringFactor::STRAIGHT; }); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } @@ -922,10 +941,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() BehaviorModuleOutput output{}; const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(); output.modified_goal = pull_over_output.modified_goal; - output.path = std::make_shared(generateStopPath()); + output.path = generateStopPath(); + output.reference_path = getPreviousModuleOutput().reference_path; const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); + output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info{}; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -1033,7 +1053,7 @@ void GoalPlannerModule::postProcess() void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) { if (prev_data_.found_path || !prev_data_.stop_path) { - prev_data_.stop_path = output.path; + prev_data_.stop_path = std::make_shared(output.path); } // for the next loop setOutput(). @@ -1065,7 +1085,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) { return; } - auto stop_path = std::make_shared(*output.path); + auto stop_path = std::make_shared(output.path); for (auto & point : stop_path->points) { point.point.longitudinal_velocity_mps = 0.0; } @@ -1761,13 +1781,13 @@ std::pair GoalPlannerModule::isSafePath() const parameters_->forward_goal_search_length); const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = - utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + 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); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - utils::start_goal_planner_common::updatePathProperty( + utils::parking_departure::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; @@ -1786,7 +1806,7 @@ std::pair GoalPlannerModule::isSafePath() const const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); - utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + utils::parking_departure::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); const double hysteresis_factor = @@ -1812,7 +1832,7 @@ std::pair GoalPlannerModule::isSafePath() const throw std::domain_error("[pull_over] invalid safety check method"); }); - /*　　 + /* *　　 ==== is_safe *　　 ---- current_is_safe *　　 is_safe @@ -1937,8 +1957,7 @@ void GoalPlannerModule::setDebugData() } add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); + utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); // visualize safety status maker { @@ -2017,21 +2036,6 @@ void GoalPlannerModule::printParkingPositionError() const distance_from_real_shoulder); } -bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const -{ - const auto & route_handler = planner_data_->route_handler; - const Pose goal_pose = route_handler->getOriginalGoalPose(); - - const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); - - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); -} - bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const { return !isOnModifiedGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp rename to planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index ee8dae231f587..b087827a9c539 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" +#include "behavior_path_goal_planner_module/goal_searcher.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp rename to planning/behavior_path_goal_planner_module/src/manager.cpp index 0916b5e8ebaef..3079361253c31 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" +#include "behavior_path_goal_planner_module/manager.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp rename to planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 9ff452b131efa..712da5aa03a4e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" +#include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/goal_planner/util.cpp rename to planning/behavior_path_goal_planner_module/src/util.cpp index 1cd985d566f73..23e9c53efae7b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include #include @@ -203,7 +203,7 @@ bool isAllowedGoalModification(const std::shared_ptr & route_handl bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); const auto shoulder_lanes = route_handler->getShoulderLanelets(); lanelet::ConstLanelet closest_shoulder_lane{}; diff --git a/planning/behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..e5bce68ebfa44 --- /dev/null +++ b/planning/behavior_path_lane_change_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/interface.cpp + src/manager.cpp + src/scene.cpp + src/utils/markers.cpp + src/utils/utils.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_lane_change_module/README.md similarity index 96% rename from planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md rename to planning/behavior_path_lane_change_module/README.md index ee74f700c4d42..a1fa2068b9ab5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -15,7 +15,7 @@ The Lane Change module is activated when lane change is needed and can be safely The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path. -![lane-change-phases](../image/lane_change/lane_change-lane_change_phases.png) +![lane-change-phases](./images/lane_change-lane_change_phases.png) ### Preparation phase @@ -62,7 +62,7 @@ Note that when the `current_velocity` is lower than `minimum_lane_changing_veloc The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. -![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) +![path_samples](./images/lane_change-candidate_path_samples.png) Which path will be chosen will depend on validity and collision check. @@ -155,21 +155,21 @@ stop #### Candidate Path's Safety check -See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md) +See [safety check utils explanation](../behavior_path_planner/docs/behavior_path_planner_safety_check.md) #### Objects selection and classification First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. -![object lanes](../image/lane_change/lane_objects.drawio.svg) +![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_planner/docs/behavior_path_planner_avoidance_design.md). ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. -![enable collision check at prepare phase](../image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png) +![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. @@ -184,7 +184,7 @@ minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_ve The following figure illustrates when the lane is blocked in multiple lane changes cases. -![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) #### Stopping position when an object exists ahead @@ -195,25 +195,25 @@ The position to be stopped depends on the situation, such as when the lane chang Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_at_terminal_no_block](../image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg) +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) -![stop_at_terminal](../image/lane_change/lane_change-stop_at_terminal.drawio.svg) +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) ##### When the ego vehicle is not near the end of the lane change If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal_no_blocking_object](../image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal](../image/lane_change/lane_change-stop_not_at_terminal.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) ##### When the target lane is far away When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. -![stop_far_from_target_lane](../image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) ### Lane Change When Stuck @@ -292,19 +292,19 @@ The function can be enabled by setting `enable_on_prepare_phase` to `true`. The following image illustrates the cancel process. -![cancel](../image/lane_change/cancel_and_abort/lane_change-cancel.png) +![cancel](./images/lane_change-cancel.png) #### Abort Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. -![abort](../image/lane_change/cancel_and_abort/lane_change-abort.png) +![abort](./images/lane_change-abort.png) #### Stop/Cruise The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. -![stop](../image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png) +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -451,11 +451,11 @@ Then add the marker in `rviz2`. -![debug](../image/lane_change/lane_change-debug-1.png) +![debug](./images/lane_change-debug-1.png) -![debug2](../image/lane_change/lane_change-debug-2.png) +![debug2](./images/lane_change-debug-2.png) -![debug3](../image/lane_change/lane_change-debug-3.png) +![debug3](./images/lane_change-debug-3.png) Available information diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/lane_change/lane_change.param.yaml rename to planning/behavior_path_lane_change_module/config/lane_change.param.yaml diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png b/planning/behavior_path_lane_change_module/images/lane_change-abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-abort.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png b/planning/behavior_path_lane_change_module/images/lane_change-cancel.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png rename to planning/behavior_path_lane_change_module/images/lane_change-cancel.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png b/planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png rename to planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-1.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-1.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-2.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-2.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-3.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-3.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png b/planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png rename to planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png b/planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png rename to planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png rename to planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png b/planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png rename to planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp similarity index 75% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 2e3cbd992e27f..fd9a979ee0f76 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,15 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -145,23 +143,6 @@ class LaneChangeInterface : public SceneModuleInterface bool is_abort_approval_requested_{false}; }; - -class AvoidanceByLaneChangeInterface : public LaneChangeInterface -{ -public: - AvoidanceByLaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map, - std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); - - bool isExecutionRequested() const override; - -protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override; -}; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp similarity index 58% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index ed476adcbd834..b34f691bd2b0d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "route_handler/route_handler.hpp" @@ -23,7 +23,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -72,46 +71,6 @@ class LaneChangeLeftModuleManager : public LaneChangeModuleManager { } }; - -class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeRightModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_right", route_handler::Direction::RIGHT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; - -class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeLeftModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_left", route_handler::Direction::LEFT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; - -class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager -{ -public: - AvoidanceByLaneChangeModuleManager() - : LaneChangeModuleManager( - "avoidance_by_lc", route_handler::Direction::NONE, - LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) - { - } - - void init(rclcpp::Node * node) override; - - std::unique_ptr createNewSceneModuleInstance() override; - -private: - std::shared_ptr avoidance_parameters_; -}; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 61caf7b2b393f..50766621b2f44 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" #include #include @@ -180,4 +180,4 @@ class NormalLaneChange : public LaneChangeBase double stop_time_{0.0}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index be49ec5b2e740..aa3ee0dc4c98b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -11,12 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -107,14 +107,13 @@ class LaneChangeBase virtual bool specialExpiredCheck() const { return false; } virtual void setPreviousModulePaths( - const std::shared_ptr & prev_module_reference_path, - const std::shared_ptr & prev_module_path) + const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path) { - if (prev_module_reference_path) { - prev_module_reference_path_ = *prev_module_reference_path; + if (!prev_module_reference_path.points.empty()) { + prev_module_reference_path_ = prev_module_reference_path; } - if (prev_module_path) { - prev_module_path_ = *prev_module_path; + if (!prev_module_path.points.empty()) { + prev_module_path_ = prev_module_path; } }; @@ -265,8 +264,8 @@ class LaneChangeBase mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp similarity index 69% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index f11d48a24c79c..dd7760d31eaa7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -11,18 +11,61 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include #include +#include #include namespace behavior_path_planner { +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; struct LaneChangeCancelParameters { @@ -33,6 +76,7 @@ struct LaneChangeCancelParameters double max_lateral_jerk{10.0}; double overhang_tolerance{0.0}; }; + struct LaneChangeParameters { // trajectory generation @@ -41,6 +85,15 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // lane change parameters + double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; + double lane_changing_lateral_jerk{0.5}; + double minimum_lane_changing_velocity{5.6}; + double lane_change_prepare_duration{4.0}; + double lane_change_finish_judge_buffer{3.0}; + LateralAccelerationMap lane_change_lat_acc_map; + // parked vehicle double object_check_min_road_shoulder_width{0.5}; double object_shiftable_ratio_threshold{0.6}; @@ -147,15 +200,6 @@ enum class LaneChangeModuleType { EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; - -struct AvoidanceByLCParameters : public AvoidanceParameters -{ - // execute only when the target object longitudinal distance is larger than this param. - double execute_object_longitudinal_margin{0.0}; - - // execute only when lane change end point is before the object. - bool execute_only_when_lane_change_finish_before_object{false}; -}; } // namespace behavior_path_planner namespace behavior_path_planner::data::lane_change @@ -167,4 +211,4 @@ struct PathSafetyStatus }; } // namespace behavior_path_planner::data::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 96f0851ed33c0..d6deecd4f46fa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include + #include -#include #include namespace marker_utils::lane_change_markers @@ -38,4 +38,4 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index c98f62c47860c..9e42b49635b82 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -50,4 +50,4 @@ struct LaneChangeStatus }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp similarity index 85% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 12e6da00b2f36..67506326d92aa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "rclcpp/logger.hpp" #include @@ -54,17 +55,21 @@ using tier4_autoware_utils::Polygon2d; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, + const double length_to_intersection = 0.0); + double calcMaximumLaneChangeLength( - const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, - const BehaviorPathPlannerParameters & params); + const LaneChangeParameters & lane_change_parameters); double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); double calcLaneChangingAcceleration( const double initial_lane_changing_velocity, const double max_path_velocity, @@ -130,7 +135,7 @@ double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & curent_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param, const Direction direction); + const LaneChangeParameters & lane_change_parameters, const Direction direction); lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -150,7 +155,8 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameters, const double resolution); + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution); bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, @@ -202,5 +208,17 @@ lanelet::ConstLanelets generateExpandedLanelets( const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, const double right_offset); +/** + * @brief Retrieves a logger instance for a specific lane change type. + * + * This function provides a specialized logger for different types of lane change. + * + * @param type A string representing the type of lane change operation. This could be + * a specific maneuver or condition related to lane changing, such as + * 'avoidance_by_lane_change', 'normal', 'external_request'. + * + * @return rclcpp::Logger The logger instance configured for the specified lane change type. + */ +rclcpp::Logger getLogger(const std::string & type); } // namespace behavior_path_planner::utils::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/package.xml b/planning/behavior_path_lane_change_module/package.xml new file mode 100644 index 0000000000000..fb4bef9535a48 --- /dev/null +++ b/planning/behavior_path_lane_change_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_lane_change_module + 0.1.0 + The behavior_path_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..a598bc8176938 --- /dev/null +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp similarity index 85% rename from planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp rename to planning/behavior_path_lane_change_module/src/interface.cpp index 413350e3eb23b..00793532023b9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/markers.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" @@ -26,7 +26,6 @@ #include #include #include -#include namespace behavior_path_planner { @@ -45,6 +44,7 @@ LaneChangeInterface::LaneChangeInterface( prev_approved_path_{std::make_unique()} { steering_factor_interface_ptr_ = std::make_unique(&node, name); + logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } void LaneChangeInterface::processOnEntry() @@ -79,8 +79,7 @@ bool LaneChangeInterface::isExecutionReady() const ModuleStatus LaneChangeInterface::updateState() { auto log_warn_throttled = [&](const std::string & message) -> void { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, message); + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 5000, message); }; if (module_type_->specialExpiredCheck()) { @@ -112,8 +111,7 @@ ModuleStatus LaneChangeInterface::updateState() if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) { RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Ego stopped at traffic light. Canceling lane change"); + getLogger(), *clock_, 5000, "Ego stopped at traffic light. Canceling lane change"); module_type_->toCancelState(); return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; } @@ -148,8 +146,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - const auto & common_parameters = module_type_->getCommonParam(); - const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane; + const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane; const auto status = module_type_->getLaneChangeStatus(); if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) { log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change."); @@ -203,8 +200,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); - path_reference_ = output.reference_path; - *prev_approved_path_ = *getPreviousModuleOutput().path; + path_reference_ = std::make_shared(output.reference_path); + *prev_approved_path_ = getPreviousModuleOutput().path; stop_pose_ = module_type_->getStopPose(); @@ -221,12 +218,12 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = *getPreviousModuleOutput().path; + *prev_approved_path_ = getPreviousModuleOutput().path; module_type_->insertStopPoint( module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = std::make_shared(*prev_approved_path_); + out.path = *prev_approved_path_; out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; @@ -242,9 +239,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() } // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); + out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); stop_pose_ = module_type_->getStopPose(); @@ -368,11 +365,10 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); const auto start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_position, status.lane_change_path.info.shift_line.start.position); + output.path.points, current_position, status.lane_change_path.info.shift_line.start.position); const auto finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_position, status.lane_change_path.info.shift_line.end.position); + output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction, @@ -433,8 +429,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & common_parameter = module_type_->getCommonParam(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane); + const double next_lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double & base_to_front = common_parameter.base_link2front; @@ -474,38 +470,4 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( // not in the vicinity of the end of the path. return original return original_turn_signal_info; } - -AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map, - std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: LaneChangeInterface{ - name, - node, - parameters, - rtc_interface_ptr_map, - objects_of_interest_marker_interface_ptr_map, - std::make_unique(parameters, avoidance_by_lane_change_parameters)} -{ -} - -bool AvoidanceByLaneChangeInterface::isExecutionRequested() const -{ - return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); -} - -void AvoidanceByLaneChangeInterface::updateRTCStatus( - const double start_distance, const double finish_distance) -{ - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); -} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp similarity index 62% rename from planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp rename to planning/behavior_path_lane_change_module/src/manager.cpp index 29b7cad533a66..e4405a31a360d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -141,6 +142,49 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // lane change parameters + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_length_buffer_for_end_of_lane = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); + p.backward_length_buffer_for_blocking_object = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.lane_changing_lateral_jerk = + getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); + p.lane_change_prepare_duration = + getOrDeclareParameter(*node, parameter("prepare_duration")); + p.minimum_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); + p.minimum_lane_changing_velocity = + std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); + p.lane_change_finish_judge_buffer = + getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); + + if (p.backward_length_buffer_for_end_of_lane < 1.0) { + RCLCPP_WARN_STREAM( + node->get_logger().get_child(name()), + "Lane change buffer must be more than 1 meter. Modifying the buffer."); + } + + // lateral acceleration map for lane change + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(name()), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.lane_change_lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + // target object { const std::string ns = "lane_change.target_object."; @@ -219,16 +263,10 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { - if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); - } return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, direction_)); + std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) @@ -246,148 +284,6 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(*node, ns + "execute_object_longitudinal_margin"); - p.execute_only_when_lane_change_finish_before_object = - getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); - } - - // general params - { - const std::string ns = "avoidance."; - p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); - p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); - return param; - }; - - const std::string ns = "avoidance_by_lane_change.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.target_filtering."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); - } - - { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); - } - - { - const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); - p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); - p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); - } - - // safety check - { - const std::string ns = "avoidance.safety_check."; - p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); - } - - avoidance_parameters_ = std::make_shared(p); -} - -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() -{ - return std::make_unique( - name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); -} - } // namespace behavior_path_planner #include @@ -397,12 +293,3 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( behavior_path_planner::LaneChangeLeftModuleManager, behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceByLaneChangeModuleManager, - behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp similarity index 93% rename from planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp rename to planning/behavior_path_lane_change_module/src/scene.cpp index 423ffc48dd6cd..f0972d66416c8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -145,38 +145,38 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() BehaviorModuleOutput output; if (isAbortState() && abort_path_) { - output.path = std::make_shared(abort_path_->path); - output.reference_path = std::make_shared(prev_module_reference_path_); + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; output.turn_signal_info = prev_turn_signal_info_; - insertStopPoint(status_.current_lanes, *output.path); + insertStopPoint(status_.current_lanes, output.path); } else { - output.path = std::make_shared(getLaneChangePath().path); + output.path = getLaneChangePath().path; const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::combinePath(*output.path, *found_extended_path); + output.path = utils::combinePath(output.path, *found_extended_path); } - output.reference_path = std::make_shared(getReferencePath()); + output.reference_path = getReferencePath(); output.turn_signal_info = updateOutputTurnSignal(); if (isStopState()) { const auto current_velocity = getEgoVelocity(); const auto current_dist = calcSignedArcLength( - output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, *output.path); + insertStopPoint(status_.target_lanes, output.path); } } extendOutputDrivableArea(output); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( - *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -189,7 +189,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *getRouteHandler(), status_.current_lanes, status_.target_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); @@ -215,9 +215,8 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, - 0.0); + const double lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -233,7 +232,7 @@ void NormalLaneChange::insertStopPoint( distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } - const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; @@ -294,15 +293,14 @@ void NormalLaneChange::insertStopPoint( if (distance_to_ego_lane_obj < distance_to_terminal) { // consider rss distance when the LC need to avoid obstacles const auto rss_dist = calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); - const double lane_change_buffer_for_blocking_object = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, - getCommonParam().backward_length_buffer_for_blocking_object, 0.0); + const double lane_change_buffer_for_blocking_object = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - - getCommonParam().backward_length_buffer_for_blocking_object - rss_dist - + lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - getCommonParam().base_link2front; // If the target lane in the lane change section is blocked by a stationary obstacle, there @@ -498,9 +496,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & current_pose = getEgoPose(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = utils::calcMinimumLaneChangeLength( - planner_data_->parameters, shift_intervals, - getCommonParam().backward_length_buffer_for_end_of_lane); + const auto lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -519,7 +516,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; const double dist_to_lane_change_end = utils::getSignedDistance( current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); - double finish_judge_buffer = planner_data_->parameters.lane_change_finish_judge_buffer; + double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer const double ego_velocity = getEgoVelocity(); @@ -560,7 +557,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const planner_data_->parameters.ego_nearest_yaw_threshold); const double ego_velocity = - std::max(getEgoVelocity(), planner_data_->parameters.minimum_lane_changing_velocity); + std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; @@ -664,10 +661,10 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; // calculate minimum and maximum acceleration - const auto min_acc = - utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto min_acc = utils::lane_change::calcMinimumAcceleration( + getEgoVelocity(), vehicle_min_acc, *lane_change_parameters_); const auto max_acc = utils::lane_change::calcMaximumAcceleration( - getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); return {min_acc, max_acc}; } @@ -678,8 +675,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength( const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); return utils::lane_change::calcMaximumLaneChangeLength( - std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(), - shift_intervals, max_acc); + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()), + *lane_change_parameters_, shift_intervals, max_acc); } std::vector NormalLaneChange::sampleLongitudinalAccValues( @@ -746,7 +743,6 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( std::vector NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - const auto & common_parameters = planner_data_->parameters; const auto base_link2front = planner_data_->parameters.base_link2front; const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; @@ -754,7 +750,7 @@ std::vector NormalLaneChange::calcPrepareDuration( std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0; + for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { @@ -1001,12 +997,7 @@ PathWithLaneId NormalLaneChange::getTargetSegment( std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); }); - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getTargetSegment"), - "start: %f, end: %f", s_start, s_end); + RCLCPP_DEBUG(logger_, "in %s start: %f, end: %f", __func__, s_start, s_end); PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); for (auto & point : target_segment.points) { @@ -1024,12 +1015,11 @@ bool NormalLaneChange::hasEnoughLength( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - const auto & common_parameters = planner_data_->parameters; const double lane_change_length = path.info.length.sum(); const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = utils::calcMinimumLaneChangeLength( - common_parameters, shift_intervals, common_parameters.backward_length_buffer_for_end_of_lane); + double minimum_lane_change_length_to_preferred_lane = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1125,7 +1115,8 @@ bool NormalLaneChange::getLaneChangePaths( 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 = common_parameters.minimum_lane_changing_velocity; + 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 @@ -1137,12 +1128,12 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), - common_parameters.backward_length_buffer_for_end_of_lane); - const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()), - common_parameters.backward_length_buffer_for_end_of_lane); + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1219,7 +1210,7 @@ bool NormalLaneChange::getLaneChangePaths( // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + 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; @@ -1238,7 +1229,7 @@ bool NormalLaneChange::getLaneChangePaths( }; const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); + 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, @@ -1264,8 +1255,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); const double backward_buffer = - num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; + num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = + lane_change_parameters_->lane_change_finish_judge_buffer; if ( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > @@ -1536,7 +1528,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const { - const auto threshold = planner_data_->parameters.backward_length_buffer_for_end_of_lane; + const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; return isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && isAbleToStopSafely() && is_object_coming_from_rear; } @@ -1546,7 +1538,7 @@ bool NormalLaneChange::calcAbortPath() const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = - std::max(common_param.minimum_lane_changing_velocity, getEgoVelocity()); + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; const auto reference_lanelets = selected_path.info.current_lanes; @@ -1557,8 +1549,8 @@ bool NormalLaneChange::calcAbortPath() const auto direction = getDirection(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( - common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1605,8 +1597,8 @@ bool NormalLaneChange::calcAbortPath() } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param, - direction)) { + *route_handler, reference_lanelets, current_pose, abort_return_dist, + *lane_change_parameters_, direction)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -1629,7 +1621,8 @@ bool NormalLaneChange::calcAbortPath() const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); - const auto lateral_acc_range = common_param.lane_change_lat_acc_map.find(current_velocity); + const auto lateral_acc_range = + lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); @@ -1662,7 +1655,7 @@ bool NormalLaneChange::calcAbortPath() PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); ref.points.back().point.longitudinal_velocity_mps = std::min( ref.points.back().point.longitudinal_velocity_mps, - static_cast(common_param.minimum_lane_changing_velocity)); + static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); return ref; }); @@ -1707,7 +1700,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const double & time_resolution = lane_change_parameters_->prediction_time_resolution; const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, common_parameters, time_resolution); + lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, + time_resolution); const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); @@ -1818,10 +1812,9 @@ bool NormalLaneChange::isVehicleStuck( : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, - 0.0); - const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + 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; if (distance_to_terminal < terminal_judge_buffer) { return true; @@ -1844,7 +1837,7 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); const auto rss_dist = calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp similarity index 98% rename from planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp rename to planning/behavior_path_lane_change_module/src/utils/markers.cpp index c2a640f989b50..719acba405a68 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,7 +16,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp similarity index 92% rename from planning/behavior_path_planner/src/utils/lane_change/utils.cpp rename to planning/behavior_path_lane_change_module/src/utils/utils.cpp index 66182454ecf85..df73990b770a3 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -67,17 +67,43 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, + const double length_to_intersection) +{ + if (shift_intervals.empty()) { + return 0.0; + } + + const double & vel = lane_change_parameters.minimum_lane_changing_velocity; + const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); + const double & max_lateral_acc = lat_acc.second; + const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const double & backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + + double accumulated_length = length_to_intersection; + for (const auto & shift_interval : shift_intervals) { + const double t = + PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); + accumulated_length += vel * t + finish_judge_buffer; + } + accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); + + return accumulated_length; +} + double calcMaximumLaneChangeLength( - const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) { if (shift_intervals.empty()) { return 0.0; } - const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - const double & lateral_jerk = common_param.lane_changing_lateral_jerk; - const double & t_prepare = common_param.lane_change_prepare_duration; + const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const double & t_prepare = lane_change_parameters.lane_change_prepare_duration; double vel = current_velocity; double accumulated_length = 0.0; @@ -87,7 +113,7 @@ double calcMaximumLaneChangeLength( vel = vel + max_acc * t_prepare; // lane changing section - const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); + const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); const double t_lane_changing = PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, lat_acc.second); const double lane_changing_length = @@ -97,26 +123,26 @@ double calcMaximumLaneChangeLength( vel = vel + max_acc * t_lane_changing; } accumulated_length += - common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + lane_change_parameters.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); return accumulated_length; } double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, - const BehaviorPathPlannerParameters & params) + const LaneChangeParameters & lane_change_parameters) { - const double min_lane_changing_velocity = params.minimum_lane_changing_velocity; - const double prepare_duration = params.lane_change_prepare_duration; + const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); } double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) { - const double prepare_duration = params.lane_change_prepare_duration; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; const double acc = (current_max_velocity - current_velocity) / prepare_duration; return std::clamp(acc, 0.0, max_longitudinal_acc); } @@ -218,7 +244,9 @@ lanelet::ConstLanelets getTargetNeighborLanes( for (const auto & current_lane : current_lanes) { if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { neighbor_lanes.push_back(current_lane); } } else { @@ -601,12 +629,12 @@ double getLateralShift(const LaneChangePath & path) bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param, const Direction direction) + const LaneChangeParameters & lane_change_parameters, const Direction direction) { const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( - common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); + const double minimum_lane_change_length = + calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -756,7 +784,9 @@ std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { return route_handler.getLaneChangeTarget(current_lanes, direction); } @@ -765,7 +795,8 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const BehaviorPathPlannerParameters & common_parameters, const double resolution) + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution) { if (lane_change_path.path.points.empty()) { return {}; @@ -776,7 +807,8 @@ std::vector convertToPredictedPath( const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; const auto duration = lane_change_path.info.duration.sum(); const auto prepare_time = lane_change_path.info.duration.prepare; - const auto & minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; + const auto & minimum_lane_changing_velocity = + lane_change_parameters.minimum_lane_changing_velocity; const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, @@ -1065,7 +1097,8 @@ std::optional createPolygon( } ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, + const PredictedObject & object, + [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object; @@ -1078,7 +1111,7 @@ ExtendedPredictedObject transform( const auto & time_resolution = lane_change_parameters.prediction_time_resolution; const auto & check_at_prepare_phase = lane_change_parameters.enable_prepare_segment_collision_check; - const auto & prepare_duration = common_parameters.lane_change_prepare_duration; + const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; @@ -1130,4 +1163,9 @@ lanelet::ConstLanelets generateExpandedLanelets( const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); } + +rclcpp::Logger getLogger(const std::string & type) +{ + return rclcpp::get_logger("lane_change").get_child(type); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..7a606b5d126ce --- /dev/null +++ b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp similarity index 96% rename from planning/behavior_path_planner/test/test_lane_change_utils.cpp rename to planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 3268bd7ec468e..1f90114df8045 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include #include @@ -41,7 +41,7 @@ TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) TEST(BehaviorPathPlanningLaneChangeUtilsTest, TESTLateralAccelerationMap) { - LateralAccelerationMap lat_acc_map; + behavior_path_planner::LateralAccelerationMap lat_acc_map; lat_acc_map.add(0.0, 0.2, 0.315); lat_acc_map.add(3.0, 0.2, 0.315); lat_acc_map.add(5.0, 0.2, 0.315); diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 2ea43bf5a4ef1..f3727f35a492f 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -12,40 +12,8 @@ pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/avoidance/manager.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/start_planner/start_planner_module.cpp - src/scene_module/start_planner/manager.cpp - src/scene_module/goal_planner/goal_planner_module.cpp - src/scene_module/goal_planner/manager.cpp - src/scene_module/side_shift/side_shift_module.cpp - src/scene_module/side_shift/manager.cpp - src/scene_module/lane_change/interface.cpp - src/scene_module/lane_change/normal.cpp - src/scene_module/lane_change/external_request.cpp - src/scene_module/lane_change/avoidance_by_lane_change.cpp - src/scene_module/lane_change/manager.cpp - src/utils/start_goal_planner_common/utils.cpp - src/utils/avoidance/shift_line_generator.cpp - src/utils/avoidance/utils.cpp - src/utils/lane_change/utils.cpp - src/utils/side_shift/util.cpp - src/utils/goal_planner/util.cpp - src/utils/goal_planner/shift_pull_over.cpp - src/utils/goal_planner/geometric_pull_over.cpp - src/utils/goal_planner/freespace_pull_over.cpp - src/utils/goal_planner/goal_searcher.cpp - src/utils/goal_planner/default_fixed_goal_planner.cpp - src/utils/start_planner/util.cpp - src/utils/start_planner/shift_pull_out.cpp - src/utils/start_planner/geometric_pull_out.cpp - src/utils/start_planner/freespace_pull_out.cpp - src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp - src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/avoidance/debug.cpp - src/marker_utils/lane_change/debug.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC @@ -71,22 +39,6 @@ if(BUILD_TESTING) ${PROJECT_NAME}_lib ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_avoidance_module - test/test_avoidance_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_avoidance_module - ${PROJECT_NAME}_lib - ) - - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module - test/test_lane_change_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module - ${PROJECT_NAME}_lib - ) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface test/test_${PROJECT_NAME}_node_interface.cpp ) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 884767315ac12..40a23a09030de 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -139,7 +139,6 @@ Enabling and disabling the modules in the behavior path planner is primarily man The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. -- `use_experimental_lane_change_function`: Set to `true` to enable experimental features in the lane change module. !!! note diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index e37ef27d44426..36106f32158b9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -143,15 +143,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left{false}; bool should_be_avoided{false}; + std::vector ref_path_points_for_obj_poly; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided) + const bool arg_is_collision_left, const bool arg_should_be_avoided, + const std::vector & arg_ref_path_points_for_obj_poly) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; + ref_path_points_for_obj_poly = arg_ref_path_points_for_obj_poly; } }; @@ -245,11 +248,13 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateObject( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided) + const bool should_be_avoided, + const std::vector & ref_path_points_for_obj_poly) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + ref_path_points_for_obj_poly); } } @@ -307,12 +312,16 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); + void updateRefPathBeforeLaneChange(const std::vector & ego_ref_path_points); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; + bool willObjectBeOutsideEgoChangingPath( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; double calcTimeToCollision( @@ -323,18 +332,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; - double calcValidStartLengthToAvoid( + double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & input_ref_path_points, + const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double time_to_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( - const std::vector & input_ref_path_points, const Polygon2d & obj_points, - const double obj_vel, const bool is_collision_left, const double obj_normal_vel, - const std::optional & prev_object) const; + const std::vector & ref_path_points_for_obj_poly, + const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, + const double obj_normal_vel, const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; @@ -353,7 +362,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::vector target_objects_; // std::vector prev_target_objects_; - std::vector prev_input_ref_path_points; + std::optional> prev_input_ref_path_points_{std::nullopt}; + std::optional> ref_path_before_lane_change_{std::nullopt}; std::shared_ptr parameters_; TargetObjectsManager target_objects_manager_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index c02ee88d3fa3e..9e08f309a78b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -44,6 +44,8 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + private: std::shared_ptr parameters_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp deleted file mode 100644 index 98d52b79e2edf..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2021-2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ - -namespace behavior_path_planner -{ - -struct LaneFollowingParameters -{ - double lane_change_prepare_duration{0.0}; - - // finding closest lanelet - double distance_threshold{0.0}; - double yaw_threshold{0.0}; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 5ca86ee5a47b6..0ca7badd0b3f4 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -7,28 +7,19 @@ Zulfaqar Azmi - - Kosuke Takeuchi - - Kosuke Takeuchi Fumiya Watanabe Kyoichi Sugahara - - Takamasa Horibe - Satoshi Ota - - Zulfaqar Azmi - Satoshi Ota - Mamoru Sobue - - - Takayuki Murooka - Taiki Tanaka Tomoya Kimura Shumpei Wakabayashi + Tomohito Ando + Takamasa Horibe + Satoshi Ota + Kosuke Takeuchi + Kyoichi Sugahara + Takayuki Murooka Apache License 2.0 diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_planner/plugins.xml index 561246fcf75e4..a16f3dd7bddee 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_planner/plugins.xml @@ -1,12 +1,3 @@ - - - - - - - - - diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 907c5226c3908..85abf774d159e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -135,6 +135,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } planner_manager_->launchScenePlugin(*this, name); } @@ -238,39 +242,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); - // lane change parameters - p.backward_length_buffer_for_end_of_lane = - declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); - p.backward_length_buffer_for_blocking_object = - declare_parameter("lane_change.backward_length_buffer_for_blocking_object"); - p.lane_changing_lateral_jerk = - declare_parameter("lane_change.lane_changing_lateral_jerk"); - p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); - p.minimum_lane_changing_velocity = - declare_parameter("lane_change.minimum_lane_changing_velocity"); - p.minimum_lane_changing_velocity = - std::min(p.minimum_lane_changing_velocity, p.max_acc * p.lane_change_prepare_duration); - p.lane_change_finish_judge_buffer = - declare_parameter("lane_change.lane_change_finish_judge_buffer"); - - // lateral acceleration map for lane change - const auto lateral_acc_velocity = - declare_parameter>("lane_change.lateral_acceleration.velocity"); - const auto min_lateral_acc = - declare_parameter>("lane_change.lateral_acceleration.min_values"); - const auto max_lateral_acc = - declare_parameter>("lane_change.lateral_acceleration.max_values"); - if ( - lateral_acc_velocity.size() != min_lateral_acc.size() || - lateral_acc_velocity.size() != max_lateral_acc.size()) { - RCLCPP_ERROR(get_logger(), "Lane change lateral acceleration map has invalid size."); - exit(EXIT_FAILURE); - } - for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { - p.lane_change_lat_acc_map.add( - lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); - } - p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = @@ -297,10 +268,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { - RCLCPP_WARN_STREAM( - get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); - } return p; } @@ -771,7 +738,8 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( { // TODO(Horibe) do some error handling when path is not available. - auto path = output.path ? output.path : planner_data->prev_output_path; + auto path = !output.path.points.empty() ? std::make_shared(output.path) + : planner_data->prev_output_path; path->header = planner_data->route_handler->getRouteHeader(); path->header.stamp = this->now(); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c867bffc8e9bd..e4bfff8fc63b8 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -198,7 +198,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da void PlannerManager::generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const { - if (!output.path || output.path->points.empty()) { + if (output.path.points.empty()) { RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!"); return; } @@ -206,20 +206,20 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); + const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path.points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (epsilon < std::abs(di.drivable_margin)) { // for single free space pull over utils::generateDrivableArea( - *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); + output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, + output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, is_driving_forward); } else { - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); const auto & dp = data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -228,19 +228,19 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( - *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, + output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, is_driving_forward); } // extract obstacles from drivable area - utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles); + utils::extractObstaclesFromDrivableArea(output.path, di.obstacles); } std::vector PlannerManager::getRequestModules( const BehaviorModuleOutput & previous_module_output) const { - if (!previous_module_output.path) { + if (previous_module_output.path.points.empty()) { RCLCPP_ERROR_STREAM( logger_, "Current module output is null. Skip candidate module check." << "\n - Approved module list: " << getNames(approved_module_ptrs_) @@ -751,9 +751,11 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname().find("lane_change") != std::string::npos; }); + const auto success_lane_change = + std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) { + return m->name().find("lane_change") != std::string::npos || + m->name().find("avoidance_by_lc") != std::string::npos; + }); if (success_lane_change) { root_lanelet_ = updateRootLanelet(data); @@ -837,9 +839,11 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) // when lane change module is running, don't update root lanelet. const bool is_lane_change_running = std::invoke([&]() { - const auto lane_change_itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); + const auto lane_change_itr = + std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { + return m->name().find("lane_change") != std::string::npos || + m->name().find("avoidance_by_lc") != std::string::npos; + }); return lane_change_itr != approved_module_ptrs_.end(); }); if (is_lane_change_running) { diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index de76166e068d3..eb6724a453f1e 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -137,7 +137,26 @@ double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & s return max_length_to_point; } - throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); + throw std::logic_error("The shape type is not supported in dynamic_avoidance."); +} + +double calcObstacleWidth(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return shape.dimensions.y; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + throw std::logic_error("The shape type is not supported in dynamic_avoidance."); } double calcDiffAngleAgainstPath( @@ -264,12 +283,12 @@ bool DynamicAvoidanceModule::isExecutionRequested() const RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); const auto input_path = getPreviousModuleOutput().path; - if (!input_path || input_path->points.size() < 2) { + if (input_path.points.size() < 2) { return false; } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(input_path->points); + const auto is_driving_forward = motion_utils::isDrivingForward(input_path.points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -401,9 +420,11 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto input_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + const auto input_ref_path_points = getPreviousModuleOutput().reference_path.points; const auto prev_objects = target_objects_manager_.getValidObjects(); + updateRefPathBeforeLaneChange(input_ref_path_points); + // 1. Rough filtering of target objects target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { @@ -427,7 +448,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(input_path->points, predicted_object); + projectObstacleVelocityToTrajectory(input_path.points, predicted_object); if ( std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { @@ -435,7 +456,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleBetweenPaths(input_path->points, obj_path); + const double obj_angle = calcDiffAngleBetweenPaths(input_path.points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -455,7 +476,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.e. check if object lateral offset to ego's path is small enough - const double obj_dist_to_path = calcDistanceToPath(input_path->points, obj_pose.position); + const double obj_dist_to_path = calcDistanceToPath(input_path.points, obj_pose.position); const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { RCLCPP_INFO_EXPRESSION( @@ -498,8 +519,10 @@ void DynamicAvoidanceModule::updateTargetObjects() object.predicted_paths.begin(), object.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const auto & ref_path_points_for_obj_poly = input_path.points; + // 2.a. check if object is not to be followed by ego - const double obj_angle = calcDiffAngleAgainstPath(input_path->points, object.pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, object.pose); const bool is_object_aligned_to_path = std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); @@ -513,13 +536,13 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.b. calculate which side object exists against ego's path - const bool is_object_left = isLeft(input_path->points, object.pose.position); + const bool is_object_left = isLeft(input_path.points, object.pose.position); const auto lat_lon_offset = - getLateralLongitudinalOffset(input_path->points, object.pose, object.shape); + getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); // 2.c. check if object will not cut in const bool will_object_cut_in = - willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset); + willObjectCutIn(input_path.points, obj_path, object.vel, lat_lon_offset); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -535,9 +558,20 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } + // 2.e. check if the ego will change the lane and the object will be outside the ego's path + // const auto will_object_be_outside_ego_changing_path = + // willObjectBeOutsideEgoChangingPath(object.pose, object.shape, object.vel); + // if (will_object_be_outside_ego_changing_path) { + // RCLCPP_INFO_EXPRESSION( + // getLogger(), parameters_->enable_debug_info, + // "[DynamicAvoidance] Ignore obstacle (%s) since the object will be outside ego's changing + // path", obj_uuid.c_str()); + // continue; + // } + // 2.e. check time to collision const double time_to_collision = - calcTimeToCollision(input_path->points, object.pose, object.vel, lat_lon_offset); + calcTimeToCollision(input_path.points, object.pose, object.vel, lat_lon_offset); if ( (0 <= object.vel && parameters_->max_time_to_collision_overtaking_object < time_to_collision) || @@ -561,13 +595,13 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); const bool is_collision_left = - future_obj_pose ? isLeft(input_path->points, future_obj_pose->position) : is_object_left; + future_obj_pose ? isLeft(input_path.points, future_obj_pose->position) : is_object_left; // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path->points); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - input_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -590,10 +624,10 @@ void DynamicAvoidanceModule::updateTargetObjects() // "ego_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - input_ref_path_points, object.pose, obj_points, object.vel, obj_path, object.shape, + ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_to_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - input_ref_path_points, obj_points, object.vel, is_collision_left, object.lat_vel, + ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, prev_object); if (!lat_offset_to_avoid) { RCLCPP_INFO_EXPRESSION( @@ -606,10 +640,37 @@ void DynamicAvoidanceModule::updateTargetObjects() const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided); + obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, + ref_path_points_for_obj_poly); } - prev_input_ref_path_points = input_ref_path_points; + prev_input_ref_path_points_ = input_ref_path_points; +} + +void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( + const std::vector & ego_ref_path_points) +{ + if (ref_path_before_lane_change_) { + // check if the ego is close enough to the current ref path, meaning that lane change ends. + const auto ego_pos = getEgoPose().position; + const double dist_to_ref_path = + std::abs(motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); + + constexpr double epsilon_dist_to_ref_path = 0.5; + if (dist_to_ref_path < epsilon_dist_to_ref_path) { + ref_path_before_lane_change_ = std::nullopt; + } + } else { + // check if the ego is during lane change. + if (prev_input_ref_path_points_ && !prev_input_ref_path_points_->empty()) { + const double dist_ref_paths = std::abs(motion_utils::calcLateralOffset( + ego_ref_path_points, prev_input_ref_path_points_->front().point.pose.position)); + constexpr double epsilon_ref_paths_diff = 1.0; + if (epsilon_ref_paths_diff < dist_ref_paths) { + ref_path_before_lane_change_ = *prev_input_ref_path_points_; + } + } + } } [[maybe_unused]] std::optional> @@ -765,6 +826,25 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return DecisionWithReason{false}; } +[[maybe_unused]] bool DynamicAvoidanceModule::willObjectBeOutsideEgoChangingPath( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const +{ + if (!ref_path_before_lane_change_ || obj_vel < 0.0) { + return false; + } + + // Check if object is in the lane before ego's lane change. + const double dist_to_ref_path_before_lane_change = + std::abs(motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); + const double epsilon_dist_checking_in_lane = calcObstacleWidth(obj_shape); + if (epsilon_dist_checking_in_lane < dist_to_ref_path_before_lane_change) { + return false; + } + + return true; +} + std::pair DynamicAvoidanceModule::getAdjacentLanes( const double forward_distance, const double backward_distance) const { @@ -840,13 +920,13 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi } MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & input_ref_path_points, + const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double time_to_collision) const { const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, obj_pose.position); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { @@ -854,7 +934,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - input_ref_path_points, obj_seg_idx, geom_obj_point); + ref_path_points_for_obj_poly, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -882,23 +962,27 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object : parameters_->end_duration_to_avoid_oncoming_object); - if (obj_vel < 0) { - const double valid_start_length_to_avoid = - calcValidStartLengthToAvoid(obj_path, obj_pose, obj_shape); + const double valid_length_to_avoid = calcValidLengthToAvoid(obj_path, obj_pose, obj_shape); + if (obj_vel < -0.5) { return MinMaxValue{ - std::max(obj_lon_offset.min_value - start_length_to_avoid, valid_start_length_to_avoid), + std::max(obj_lon_offset.min_value - start_length_to_avoid, -valid_length_to_avoid), obj_lon_offset.max_value + end_length_to_avoid}; } + if (0.5 < obj_vel) { + return MinMaxValue{ + obj_lon_offset.min_value - start_length_to_avoid, + std::min(obj_lon_offset.max_value + end_length_to_avoid, valid_length_to_avoid)}; + } return MinMaxValue{ obj_lon_offset.min_value - start_length_to_avoid, obj_lon_offset.max_value + end_length_to_avoid}; } -double DynamicAvoidanceModule::calcValidStartLengthToAvoid( +double DynamicAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const { - const auto input_path_points = getPreviousModuleOutput().path->points; + const auto input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); @@ -926,31 +1010,33 @@ double DynamicAvoidanceModule::calcValidStartLengthToAvoid( } return obj_path.path.size() - 1; }(); - return -motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); + return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); } std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( - const std::vector & input_ref_path_points, const Polygon2d & obj_points, - const double obj_vel, const bool is_collision_left, const double obj_normal_vel, - const std::optional & prev_object) const + const std::vector & ref_path_points_for_obj_poly, + const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, + const double obj_normal_vel, const std::optional & prev_object) const { + const bool enable_lowpass_filter = true; + /* const bool enable_lowpass_filter = [&]() { - if (prev_input_ref_path_points.size() < 2) { + if (!prev_ref_path_points_for_obj_poly_ || prev_ref_path_points_for_obj_poly_->size() < 2) { return true; } const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex( - prev_input_ref_path_points, input_ref_path_points.front().point.pose.position); - constexpr double min_lane_change_path_lat_offset = 1.0; - if ( - motion_utils::calcLateralOffset( - prev_input_ref_path_points, input_ref_path_points.front().point.pose.position, - prev_front_seg_idx) < min_lane_change_path_lat_offset) { - return true; + *prev_ref_path_points_for_obj_poly_, + ref_path_points_for_obj_poly.front().point.pose.position); constexpr double + min_lane_change_path_lat_offset = 1.0; if ( motion_utils::calcLateralOffset( + *prev_ref_path_points_for_obj_poly_, + ref_path_points_for_obj_poly.front().point.pose.position, prev_front_seg_idx) < + min_lane_change_path_lat_offset) { return true; } // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to // shift the obstacle polygon suddenly. return false; }(); + */ // calculate min/max lateral offset from object to path const auto obj_lat_abs_offset = [&]() { @@ -958,9 +1044,9 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, geom_obj_point); - const double obj_point_lat_offset = - motion_utils::calcLateralOffset(input_ref_path_points, geom_obj_point, obj_point_seg_idx); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point); + const double obj_point_lat_offset = motion_utils::calcLateralOffset( + ref_path_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } return getMinMaxValues(obj_lat_abs_offset_vec); @@ -1027,19 +1113,19 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( return std::nullopt; } - auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + auto ref_path_points_for_obj_poly = object.ref_path_points_for_obj_poly; const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, object.pose.position); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid->min_value, input_ref_path_points); + obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, input_ref_path_points); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_path_points_for_obj_poly); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -1049,20 +1135,20 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(input_ref_path_points.size() - 1); + : static_cast(ref_path_points_for_obj_poly.size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( - input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0) - .position); - obj_outer_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( - input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->max_value, 0.0) - .position); + obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + ref_path_points_for_obj_poly.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->min_value, 0.0) + .position); + obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + ref_path_points_for_obj_poly.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->max_value, 0.0) + .position); } // create obj_polygon from inner/outer bound points diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 2824a6221591a..ca0bc070d0fdb 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -255,6 +255,11 @@ void DynamicAvoidanceModuleManager::updateModuleParams( if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } + +bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const +{ + return true; +} } // namespace behavior_path_planner #include diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 9235e8e8e3de6..f2c22f774fc77 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -49,16 +49,7 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); - module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); - module_names.emplace_back("behavior_path_planner::StartPlannerModuleManager"); - module_names.emplace_back("behavior_path_planner::GoalPlannerModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -72,13 +63,7 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", - behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", - behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", - behavior_path_planner_dir + "/config/start_planner/start_planner.param.yaml", - behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", - behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", - behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); + behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner_common/CMakeLists.txt index 5567f04d5726b..d1996bf72037f 100644 --- a/planning/behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner_common/CMakeLists.txt @@ -21,6 +21,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp + src/utils/parking_departure/geometric_parallel_parking.cpp + src/utils/parking_departure/utils.cpp + src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp ) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 3dcfdcc2bdcef..d52906ef4684f 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -119,10 +119,10 @@ struct BehaviorModuleOutput BehaviorModuleOutput() = default; // path planed by module - PlanResult path{}; + PathWithLaneId path{}; // reference path planed by module - PlanResult reference_path{}; + PathWithLaneId reference_path{}; TurnSignalInfo turn_signal_info{}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index a5e2c3245f3c3..62dc690e6ba28 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -362,8 +362,6 @@ class SceneModuleInterface std::string name_; - rclcpp::Logger logger_; - BehaviorModuleOutput previous_module_output_; StopReason stop_reason_; @@ -405,7 +403,7 @@ class SceneModuleInterface virtual BehaviorModuleOutput planWaitingApproval() { path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return getPreviousModuleOutput(); } @@ -581,6 +579,8 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 7832dd4e53cfc..5bdd0a2f3f88d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#include #include #include @@ -32,47 +31,6 @@ struct ModuleConfigParameters uint8_t max_module_size{0}; }; -struct LateralAccelerationMap -{ - std::vector base_vel; - std::vector base_min_acc; - std::vector base_max_acc; - - void add(const double velocity, const double min_acc, const double max_acc) - { - if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { - return; - } - - size_t idx = 0; - for (size_t i = 0; i < base_vel.size(); ++i) { - if (velocity < base_vel.at(i)) { - break; - } - idx = i + 1; - } - - base_vel.insert(base_vel.begin() + idx, velocity); - base_min_acc.insert(base_min_acc.begin() + idx, min_acc); - base_max_acc.insert(base_max_acc.begin() + idx, max_acc); - } - - std::pair find(const double velocity) const - { - if (!base_vel.empty() && velocity < base_vel.front()) { - return std::make_pair(base_min_acc.front(), base_max_acc.front()); - } - if (!base_vel.empty() && velocity > base_vel.back()) { - return std::make_pair(base_min_acc.back(), base_max_acc.back()); - } - - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); - - return std::make_pair(min_acc, max_acc); - } -}; - struct BehaviorPathPlannerParameters { bool verbose; @@ -81,8 +39,6 @@ struct BehaviorPathPlannerParameters double backward_path_length; double forward_path_length; - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; @@ -90,13 +46,6 @@ struct BehaviorPathPlannerParameters double min_acc; double max_acc; - // lane change parameters - double lane_changing_lateral_jerk{0.5}; - double minimum_lane_changing_velocity{5.6}; - double lane_change_prepare_duration{4.0}; - double lane_change_finish_judge_buffer{3.0}; - LateralAccelerationMap lane_change_lat_acc_map; - double minimum_pull_over_length; double minimum_pull_out_length; double drivable_area_resolution; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 91d99fcd4a0d6..c24aa021cf57c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #include @@ -149,4 +149,4 @@ class OccupancyGridBasedCollisionDetector } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 84d84e0628cad..c270a1500431c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -44,4 +44,4 @@ struct StartGoalPlannerData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 7f5eee7a12aaa..4d1f1fdb959c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" @@ -150,4 +150,4 @@ class GeometricParallelParking }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp similarity index 63% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp index 16be48b5acc6d..1d4245bfec0e4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -12,14 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -28,10 +25,9 @@ #include #include -namespace behavior_path_planner::utils::start_goal_planner_common +namespace behavior_path_planner::utils::parking_departure { -using behavior_path_planner::StartPlannerParameters; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -59,30 +55,6 @@ void modifyVelocityByDirection( std::vector> & terminal_vel_acc_pairs, const double target_velocity, const double acceleration); -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params); - -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params); - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params); - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params); - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params); - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params); - void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel); @@ -103,6 +75,14 @@ std::optional generateFeasibleStopPath( geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, const double maximum_jerk); -} // namespace behavior_path_planner::utils::start_goal_planner_common +/** + * @brief calculate end arc length to generate reference path considering the goal position + * @return a pair of s_end and terminal_is_goal + */ +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose); + +} // namespace behavior_path_planner::utils::parking_departure -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index 3a7d346849e16..e2c9fd1e332a2 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -333,10 +333,6 @@ lanelet::ConstLanelets calcLaneAroundPose( bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); -double calcMinimumLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double backward_buffer, const double length_to_intersection = 0.0); - lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index f6344b94a6bc5..af4bebe183d1d 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1183,6 +1183,8 @@ std::vector getBoundWithHatchedRoadMarkings( } else { if (!polygon) { will_close_polygon = true; + } else if (polygon.value().id() != current_polygon.value().id()) { + will_close_polygon = true; } else { current_polygon_border_indices.push_back( get_corresponding_polygon_index(*current_polygon, bound_point.id())); @@ -1217,6 +1219,17 @@ std::vector getBoundWithHatchedRoadMarkings( (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); } } + + if (polygon.has_value() && current_polygon.has_value()) { + if (polygon.value().id() != current_polygon.value().id()) { + current_polygon = polygon; + current_polygon_border_indices.clear(); + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(current_polygon.value(), bound_point.id())); + continue; + } + } + current_polygon = std::nullopt; current_polygon_border_indices.clear(); } diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp rename to planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 0f87c68e38289..1aab58ac2ff6e 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp rename to planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index e6dbd2679db3c..7ba4f114328a7 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -281,7 +281,7 @@ bool GeometricParallelParking::planPullOut( // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; - const auto path_end_info = start_planner_utils::calcEndArcLength( + const auto path_end_info = utils::parking_departure::calcEndArcLength( s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp similarity index 72% rename from planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp rename to planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp index cbc6d6a4a0b8d..58fd392988ad9 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -12,9 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -namespace behavior_path_planner::utils::start_goal_planner_common +#include "behavior_path_planner_common/utils/utils.hpp" + +#include + +namespace behavior_path_planner::utils::parking_departure { using motion_utils::calcDecelDistWithJerkAndAccConstraints; @@ -80,53 +84,6 @@ void modifyVelocityByDirection( } } -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params) -{ - ego_predicted_path_params = - std::make_shared(start_planner_params->ego_predicted_path_params); -} -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params) -{ - ego_predicted_path_params = - std::make_shared(goal_planner_params->ego_predicted_path_params); -} - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params) -{ - safety_check_params = - std::make_shared(start_planner_params->safety_check_params); -} - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params) -{ - safety_check_params = - std::make_shared(goal_planner_params->safety_check_params); -} - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params) -{ - objects_filtering_params = - std::make_shared(start_planner_params->objects_filtering_params); -} - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params) -{ - objects_filtering_params = - std::make_shared(goal_planner_params->objects_filtering_params); -} - void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) @@ -178,7 +135,7 @@ std::optional generateFeasibleStopPath( // try to insert stop point in current_path after approval // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point const auto min_stop_distance = - behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance( + behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance( planner_data, maximum_deceleration, maximum_jerk, 0.0); if (!min_stop_distance) { @@ -198,4 +155,25 @@ std::optional generateFeasibleStopPath( return current_path; } -} // namespace behavior_path_planner::utils::start_goal_planner_common +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose) +{ + const double s_forward_length = s_start + forward_path_length; + // use forward length if the goal pose is not in the lanelets + if (!utils::isInLanelets(goal_pose, road_lanes)) { + return {s_forward_length, false}; + } + + const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + + // If the goal is behind the start or beyond the forward length, use forward length. + if (s_goal < s_start || s_goal >= s_forward_length) { + return {s_forward_length, false}; + } + + // path end is goal + return {s_goal, true}; +} + +} // namespace behavior_path_planner::utils::parking_departure diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index 994a7ba1a19d5..57a5743a5963b 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -642,8 +642,8 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); BehaviorModuleOutput output; - output.path = std::make_shared(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; @@ -692,8 +692,8 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 88c7532b0ad72..7c3a5883989fb 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1515,31 +1515,6 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } -double calcMinimumLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double backward_buffer, const double length_to_intersection) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const double & vel = common_param.minimum_lane_changing_velocity; - const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); - const double & max_lateral_acc = lat_acc.second; - const double & lateral_jerk = common_param.lane_changing_lateral_jerk; - const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - - double accumulated_length = length_to_intersection; - for (const auto & shift_interval : shift_intervals) { - const double t = - PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + finish_judge_buffer; - } - accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); - - return accumulated_length; -} - lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler) { diff --git a/planning/behavior_path_side_shift_module/CMakeLists.txt b/planning/behavior_path_side_shift_module/CMakeLists.txt new file mode 100644 index 0000000000000..28009e48a4cc4 --- /dev/null +++ b/planning/behavior_path_side_shift_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_side_shift_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/utils.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml b/planning/behavior_path_side_shift_module/config/side_shift.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/side_shift/side_shift.param.yaml rename to planning/behavior_path_side_shift_module/config/side_shift.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp index 6475661f72ac4..c9edd3d6bf6a6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -51,4 +51,4 @@ struct SideShiftDebugData }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp index 53bac3914ce86..ef8a48bc29760 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "behavior_path_side_shift_module/scene.hpp" #include @@ -50,4 +50,4 @@ class SideShiftModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 014a0f403f9ec..7d72afebfb359 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_side_shift_module/data_structs.hpp" #include @@ -134,4 +134,4 @@ class SideShiftModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp index 6cdfd84d79075..210dd4e8a8d2a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #include #include @@ -41,4 +41,4 @@ Point transformToGrid( } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/behavior_path_side_shift_module/package.xml new file mode 100644 index 0000000000000..5c421b04c0e39 --- /dev/null +++ b/planning/behavior_path_side_shift_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_side_shift_module + 0.1.0 + The behavior_path_side_shift_module package + + Satoshi Ota + Fumiya Watanabe + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_side_shift_module/plugins.xml b/planning/behavior_path_side_shift_module/plugins.xml new file mode 100644 index 0000000000000..2688e3a17c2b6 --- /dev/null +++ b/planning/behavior_path_side_shift_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_side_shift_module/src/manager.cpp similarity index 97% rename from planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp rename to planning/behavior_path_side_shift_module/src/manager.cpp index b20712ef19179..fa8d579da3d09 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_side_shift_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "behavior_path_side_shift_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp similarity index 96% rename from planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp rename to planning/behavior_path_side_shift_module/src/scene.cpp index 9f77288e608cf..6df8c1ec629c9 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_side_shift_module/src/scene.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" +#include "behavior_path_side_shift_module/scene.hpp" -#include "behavior_path_planner/utils/side_shift/util.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_side_shift_module/utils.hpp" #include @@ -203,17 +203,17 @@ void SideShiftModule::updateData() ? planner_data_->self_odometry->pose.pose : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); if (prev_reference_.points.empty()) { - prev_reference_ = *getPreviousModuleOutput().path; + prev_reference_ = getPreviousModuleOutput().path; } - if (!getPreviousModuleOutput().reference_path) { + if (getPreviousModuleOutput().reference_path.points.empty()) { return; } const auto centerline_path = utils::calcCenterLinePath( planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); + getPreviousModuleOutput().reference_path); constexpr double resample_interval = 1.0; - const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path); + const auto backward_extened_path = extendBackwardLength(getPreviousModuleOutput().path); reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval); path_shifter_.setPath(reference_path_); @@ -286,7 +286,7 @@ BehaviorModuleOutput SideShiftModule::plan() output.reference_path = getPreviousModuleOutput().reference_path; prev_output_ = shifted_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); debug_data_.path_shifter = std::make_shared(path_shifter_); @@ -329,7 +329,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); prev_output_ = shifted_path; @@ -409,7 +409,8 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat { // for new architecture // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be // assigned without combining. - out.path = std::make_shared(output_path); + out.path = output_path; + out.reference_path = getPreviousModuleOutput().reference_path; out.drivable_area_info.drivable_lanes = expanded_lanes; out.drivable_area_info.is_already_expanded = true; } diff --git a/planning/behavior_path_planner/src/utils/side_shift/util.cpp b/planning/behavior_path_side_shift_module/src/utils.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/side_shift/util.cpp rename to planning/behavior_path_side_shift_module/src/utils.cpp index af733722562f8..a674d57597bc7 100644 --- a/planning/behavior_path_planner/src/utils/side_shift/util.cpp +++ b/planning/behavior_path_side_shift_module/src/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/side_shift/util.hpp" +#include "behavior_path_side_shift_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..94be083bdebcd --- /dev/null +++ b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,124 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") + + "/config/side_shift.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_start_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..6bb2f76cb9842 --- /dev/null +++ b/planning/behavior_path_start_planner_module/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_start_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/start_planner_module.cpp + src/manager.cpp + src/freespace_pull_out.cpp + src/geometric_pull_out.cpp + src/shift_pull_out.cpp + src/util.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/start_planner/start_planner.param.yaml rename to planning/behavior_path_start_planner_module/config/start_planner.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index 3a154dfd83353..f38279d172009 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" +#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include #include @@ -49,4 +49,4 @@ class FreespacePullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp similarity index 70% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 4f46350d6c2b1..164868b47535e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include @@ -36,4 +36,4 @@ class GeometricPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp index 10fcbf81f8d45..3fb64baaa2bba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "behavior_path_start_planner_module/start_planner_module.hpp" #include @@ -53,4 +53,4 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp index 9c9c6fe83e288..68fcb25cac88e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -34,4 +34,4 @@ struct PullOutPath Pose end_pose{}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 60cd2731eb92f..de3d376fa4b3d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -66,4 +66,4 @@ class PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 7a868e6c2a234..9012504ed2e7a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include @@ -57,4 +57,4 @@ class ShiftPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index f70effb7cf186..a143f34ead649 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ - -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" -#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ + #include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -100,6 +100,18 @@ class StartPlannerModule : public SceneModuleInterface void processOnExit() override; void updateData() override; + void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + + void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + + void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; @@ -244,4 +256,4 @@ class StartPlannerModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index cbde2b514366d..783aace0983ca 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -104,4 +104,4 @@ struct StartPlannerParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp similarity index 81% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 5fcb5fd5668b1..147632329d926 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -50,13 +50,6 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); -/** - * @brief calculate end arc length to generate reference path considering the goal position - * @return a pair of s_end and terminal_is_goal - */ -std::pair calcEndArcLength( - const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, - const Pose & goal_pose); } // namespace behavior_path_planner::start_planner_utils -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml new file mode 100644 index 0000000000000..cbfdba0d07a57 --- /dev/null +++ b/planning/behavior_path_start_planner_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_start_planner_module + 0.1.0 + The behavior_path_start_planner_module package + + Kosuke Takeuchi + Kyoichi Sugahara + Satoshi OTA + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_start_planner_module/plugins.xml b/planning/behavior_path_start_planner_module/plugins.xml new file mode 100644 index 0000000000000..fde8b673be5bc --- /dev/null +++ b/planning/behavior_path_start_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp similarity index 93% rename from planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp rename to planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp index 32a36e1d70509..dd068c3c26cf8 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" +#include "behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include @@ -90,8 +91,8 @@ std::optional FreespacePullOut::plan(const Pose & start_pose, const constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); - const auto path_end_info = - start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const auto path_end_info = behavior_path_planner::utils::parking_departure::calcEndArcLength( + s_start, forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp rename to planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index fefa6b440c096..7d6dd60398e8e 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp rename to planning/behavior_path_start_planner_module/src/manager.cpp index 14660396062aa..58d0fbee7921b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/start_planner/manager.hpp" +#include "behavior_path_start_planner_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp rename to planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index dd1d3d3c4cca4..73952ca02f558 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -187,8 +187,8 @@ std::vector ShiftPullOut::calcPullOutPaths( // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); - const auto path_end_info = - start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const auto path_end_info = behavior_path_planner::utils::parking_departure::calcEndArcLength( + s_start, forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp similarity index 96% rename from planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp rename to planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 7bede9192d35f..c9fa15d855268 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" +#include "behavior_path_start_planner_module/start_planner_module.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -36,7 +36,7 @@ #include #include -using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap; +using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -130,6 +130,30 @@ void StartPlannerModule::initVariables() initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } +void StartPlannerModule::updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} + +void StartPlannerModule::updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void StartPlannerModule::updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + void StartPlannerModule::updateData() { if (receivedNewRoute()) { @@ -350,7 +374,7 @@ BehaviorModuleOutput StartPlannerModule::plan() if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { auto current_path = getCurrentPath(); const auto stop_path = - behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); @@ -378,11 +402,11 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - output.path = std::make_shared(path); + output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -403,7 +427,6 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, @@ -413,7 +436,6 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); @@ -431,11 +453,9 @@ CandidateOutput StartPlannerModule::planCandidate() const void StartPlannerModule::initializeSafetyCheckParameters() { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); + updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); + updateSafetyCheckParams(safety_check_params_, parameters_); + updateObjectsFilteringParams(objects_filtering_params_, parameters_); } PathWithLaneId StartPlannerModule::getFullPath() const @@ -492,11 +512,11 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - output.path = std::make_shared(stop_path); + output.path = stop_path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_unique(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -1095,13 +1115,13 @@ bool StartPlannerModule::isSafePath() const // for ego predicted path const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); const std::pair terminal_velocity_and_accel = - utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + utils::parking_departure::getPairsTerminalVelocityAndAccel( status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); - utils::start_goal_planner_common::updatePathProperty( + utils::parking_departure::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; @@ -1122,7 +1142,7 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + utils::parking_departure::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( @@ -1162,7 +1182,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; const PathWithLaneId stop_path = generateStopPath(); - output.path = std::make_shared(stop_path); + output.path = stop_path; setDrivableAreaInfo(output); @@ -1181,7 +1201,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() } path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return output; } @@ -1238,7 +1258,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(*output.path), + output.path, generateDrivableLanes(output.path), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp similarity index 83% rename from planning/behavior_path_planner/src/utils/start_planner/util.cpp rename to planning/behavior_path_start_planner_module/src/util.cpp index 0b26c793155f7..9a9466936c422 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -106,25 +106,4 @@ lanelet::ConstLanelets getPullOutLanes( /*forward_only_in_route*/ true); } -std::pair calcEndArcLength( - const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, - const Pose & goal_pose) -{ - const double s_forward_length = s_start + forward_path_length; - // use forward length if the goal pose is not in the lanelets - if (!utils::isInLanelets(goal_pose, road_lanes)) { - return {s_forward_length, false}; - } - - const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; - - // If the goal is behind the start or beyond the forward length, use forward length. - if (s_goal < s_start || s_goal >= s_forward_length) { - return {s_forward_length, false}; - } - - // path end is goal - return {s_goal, true}; -} - } // namespace behavior_path_planner::start_planner_utils diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 9485f165849f2..1148379c26f42 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -21,7 +21,6 @@ behavior_velocity_planner_common geometry_msgs lanelet2_extension - libboost-dev motion_utils pluginlib rclcpp diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 429f6b5a76550..62a5e06674d4e 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -27,12 +27,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -203,7 +205,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return true; } -boost::optional BlindSpotModule::getFirstPointConflictingLanelets( +std::optional BlindSpotModule::getFirstPointConflictingLanelets( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & lanelets) const { @@ -229,7 +231,7 @@ boost::optional BlindSpotModule::getFirstPointConflictingLanelets( if (is_conflict) { return first_idx_conflicting_lanelets; } else { - return boost::none; + return std::nullopt; } } @@ -252,7 +254,7 @@ bool BlindSpotModule::generateStopLine( /* generate stop point */ int stop_idx_ip = 0; // stop point index for interpolated path. if (straight_lanelets.size() > 0) { - boost::optional first_idx_conflicting_lane_opt = + std::optional first_idx_conflicting_lane_opt = getFirstPointConflictingLanelets(path_ip, straight_lanelets); if (!first_idx_conflicting_lane_opt) { RCLCPP_DEBUG(logger_, "No conflicting line found."); @@ -261,7 +263,7 @@ bool BlindSpotModule::generateStopLine( stop_idx_ip = std::max( first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); } else { - boost::optional intersection_enter_point_opt = + std::optional intersection_enter_point_opt = getStartPointFromLaneLet(lane_id_); if (!intersection_enter_point_opt) { RCLCPP_DEBUG(logger_, "No intersection enter point found."); @@ -502,7 +504,7 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } -boost::optional BlindSpotModule::generateBlindSpotPolygons( +std::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, @@ -537,11 +539,21 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::ConstLanelets adjacent_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); - const auto adj = - turn_direction_ == TurnDirection::LEFT - ? (routing_graph_ptr->adjacentLeft(lane)) - : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) - : boost::none); + const auto adj = std::invoke([&]() -> std::optional { + if (turn_direction_ == TurnDirection::INVALID) { + return std::nullopt; + } + const auto adj_lane = (turn_direction_ == TurnDirection::LEFT) + ? routing_graph_ptr->adjacentLeft(lane) + : routing_graph_ptr->adjacentRight(lane); + + if (adj_lane) { + return *adj_lane; + } + + return std::nullopt; + }); + if (adj) { const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); @@ -577,7 +589,7 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( } return blind_spot_polygons; } else { - return boost::none; + return std::nullopt; } } @@ -628,13 +640,13 @@ bool BlindSpotModule::isTargetObjectType( return false; } -boost::optional BlindSpotModule::getStartPointFromLaneLet( +std::optional BlindSpotModule::getStartPointFromLaneLet( const lanelet::Id lane_id) const { lanelet::ConstLanelet lanelet = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); if (lanelet.centerline().empty()) { - return boost::none; + return std::nullopt; } const auto p = lanelet.centerline().front(); geometry_msgs::msg::Point start_point; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index bb78ed5771d2b..3915acd3532b5 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -25,8 +25,6 @@ #include #include -#include - #include #include @@ -128,7 +126,7 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return Blind spot polygons */ - boost::optional generateBlindSpotPolygons( + std::optional generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, @@ -192,7 +190,7 @@ class BlindSpotModule : public SceneModuleInterface * @param lanelets target lanelets * @return path point index */ - boost::optional getFirstPointConflictingLanelets( + std::optional getFirstPointConflictingLanelets( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & lanelets) const; @@ -201,8 +199,7 @@ class BlindSpotModule : public SceneModuleInterface * @param lane_id lane id of objective lanelet * @return end point of lanelet */ - boost::optional getStartPointFromLaneLet( - const lanelet::Id lane_id) const; + std::optional getStartPointFromLaneLet(const lanelet::Id lane_id) const; /** * @brief get straight lanelets in intersection diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 762639950b2c5..240963309e9a2 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -9,7 +9,7 @@ # param for stop position stop_position: - stop_position_threshold: 1.0 # [m] threshold to check whether the vehicle stops in front of crosswalk for yielding + stop_position_threshold: 1.0 # [m] If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. # For the Lanelet2 map with no explicit stop lines stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk @@ -18,24 +18,24 @@ # For the case where the stop position is determined according to the object position. stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin - # param for ego's slow down velocity + # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) - # param for stuck vehicles + # params to prevent stopping on crosswalks due to another vehicle ahead stuck_vehicle: enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection - stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck - max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked - stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. + max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. + stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] - # param for pass judge logic + # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) @@ -44,25 +44,26 @@ ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering - no_stop_decision: + no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal - disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal - # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + disable_stop_for_yield_cancel: true # for the crosswalks with traffic signal + disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal + # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] - timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not + timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # param for target object filtering object_filtering: crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects bicycle: true # [-] whether to look and stop by BICYCLE objects diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 3f4af4a5a1138..77de7d241c05c 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -50,6 +50,8 @@ enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; struct CollisionPoint { geometry_msgs::msg::Point collision_point{}; + std::optional crosswalk_passage_direction{ + std::nullopt}; // denote obj is passing the crosswalk along the vehicle lane double time_to_collision{}; double time_to_vehicle{}; }; diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 3830aa9edddff..9eef11e72239e 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -10,6 +10,8 @@ Shumpei Wakabayashi Takayuki Murooka Kyoichi Sugahara + Mamoru Sobue + Yuki Takagi Apache License 2.0 diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c743d440bee85..b8190cb6124e7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -116,6 +116,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for target area & object cp.crosswalk_attention_range = getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter( + node, ns + ".object_filtering.vehicle_object_cross_angle_threshold"); cp.look_unknown = getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); cp.look_bicycle = @@ -152,10 +154,10 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) path.header.stamp); }; - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { + for (const auto & crosswalk : crosswalk_reg_elem_map) { // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); } @@ -178,10 +180,10 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) crosswalk_id_set = getCrosswalkIdSetOnPath( planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { + for (const auto & crosswalk : crosswalk_reg_elem_map) { crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 239eddca1fed9..b8a34bf5a9d00 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -316,6 +317,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -349,25 +352,38 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop + auto isVehicleType = [](const uint8_t label) { + return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; + }; std::optional> nearest_stop_info; std::vector stop_factor_points; for (const auto & object : object_info_manager_.getObject()) { - const auto & collision_point = object.collision_point; - if (collision_point) { + const auto & collision_point_opt = object.collision_point; + if (collision_point_opt) { + const auto & collision_point = collision_point_opt.value(); const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } + if ( + isVehicleType(object.classification) && ego_crosswalk_passage_direction && + collision_point.crosswalk_passage_direction) { + const double direction_diff = tier4_autoware_utils::normalizeRadian( + collision_point.crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value()); + if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { + continue; + } + } stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength( - sparse_resample_path.points, ego_pos, collision_point->collision_point) - + calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { nearest_stop_info = - std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); + std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front); } } } @@ -580,6 +596,70 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } +std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( + const PathWithLaneId & path) const +{ + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.points.size() - 1; ++i) { + const auto & start = path.points.at(i).point.pose.position; + const auto & end = path.points.at(i + 1).point.pose.position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + +std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const +{ + using tier4_autoware_utils::Segment2d; + + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.path.size() - 1; ++i) { + const auto & start = path.path.at(i).position; + const auto & end = path.path.at(i + 1).position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + std::optional CrosswalkModule::getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) @@ -625,6 +705,8 @@ std::optional CrosswalkModule::getCollisionPoint( // Calculate multi-step object polygon, and reset start_idx const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto object_crosswalk_passage_direction = + findObjectPassageDirectionAlongVehicleLane(obj_path); const auto obj_multi_step_polygon = createMultiStepPolygon( obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); is_start_idx_initialized = false; @@ -660,7 +742,8 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel, + object_crosswalk_passage_direction); debug_data_.obj_polygons.push_back( toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } @@ -679,17 +762,18 @@ std::optional CrosswalkModule::getCollisionPoint( CollisionPoint CrosswalkModule::createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const { constexpr double min_ego_velocity = 1.38; // [m/s] - - const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity); CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; + collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction; collision_point.time_to_collision = std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / std::max(ego_vel.x, min_ego_velocity); @@ -931,7 +1015,8 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); + has_traffic_light, collision_point, object.classification.front().label, planner_param_, + crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index ef6d01fc43c23..7f020fbe5422c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -150,6 +150,7 @@ class CrosswalkModule : public SceneModuleInterface double traffic_light_state_timeout; // param for target area & object double crosswalk_attention_range; + double vehicle_object_cross_angle_threshold; bool look_unknown; bool look_bicycle; bool look_motorcycle; @@ -160,6 +161,7 @@ class CrosswalkModule : public SceneModuleInterface { CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; + uint8_t classification{ObjectClassification::UNKNOWN}; geometry_msgs::msg::Point position{}; std::optional collision_point{}; @@ -242,8 +244,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param, - const lanelet::BasicPolygon2d & crosswalk_polygon) + const std::optional & collision_point, const uint8_t classification, + const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -265,6 +267,7 @@ class CrosswalkModule : public SceneModuleInterface crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; + objects.at(uuid).classification = classification; } void finalize() { @@ -330,6 +333,11 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; + std::optional findEgoPassageDirectionAlongPath( + const PathWithLaneId & sparse_resample_path) const; + std::optional findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const; + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); @@ -366,7 +374,8 @@ class CrosswalkModule : public SceneModuleInterface CollisionPoint createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const; + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const; float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 12455a26d2f4a..58c2ce59edd48 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -184,6 +184,12 @@ If there is any object on the path inside the intersection and at the exit of th ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) +## Yield stuck vehicle detection + +If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection. This scene happens when the object is yielding against ego or the object is waiting before the crosswalk around the exit of the intersection. + +![yield_stuck_detection](./docs/yield-stuck.drawio.svg) + ## Collision detection The following process is performed for the targets objects to determine whether ego can pass the intersection safely. If it is judged that ego cannot pass the intersection with enough margin, this module inserts a stopline on the path. @@ -271,6 +277,8 @@ If the traffic light color is RED or Arrow signal is turned on, the attention la When the traffic light color/shape is RED/Arrow, occlusion detection is skipped. +![traffic-light-specific-behavior](./docs/traffic-light-specific-behavior.drawio.svg) + ## Pass Judge Line To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position. diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 1e6ce843de528..997addd48d7f8 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -29,9 +29,9 @@ yield_stuck: turn_direction: left: true - right: false + right: true straight: false - distance_threshold: 1.0 + distance_threshold: 5.0 collision_detection: consider_wrong_direction_vehicle: false diff --git a/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg b/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg new file mode 100644 index 0000000000000..0ba5f8e41d74b --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg @@ -0,0 +1,1536 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + with traffic light(left-hand) +
+ +
+ right +
+
+ Just after changed to +
+ GREEN +
+
+ +
+
+
+ + with traffic light(left-hand)... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + with traffic light(left-hand) +
+
+ right +
+
+ while YELLOW +
+ +
+
+
+ + with traffic light(left-hand)... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + with traffic light(left-hand) +
+
+ right +
+
+ while RED/arrow + + +
+ +
+
+
+ + with traffic light(left-hand)... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + stop for a specific duration +
+ regardless of collision detection +
+ +
+
+
+ + stop for a specific duration... + + + + + +
+
+
+ + If the vehicles are not +
+ fully accelerated around +
+ the stopline +
+ +
+
+
+ + If the vehicles are not... + + + + + + + + + +
+
+
+ + relaxed collision checking +
+ compared to GREEN +
+ +
+
+
+ + relaxed collision checking... + + + + + +
+
+
+ + If the vehicle is expected to +
+ stop before the stopline under +
+ the deceleration, ignore it +
+
+ Otherwise check collision +
+ +
+
+
+ + If the vehicle is expected to... + + + + + + + + + + +
+
+
+ + more relaxed collision +
+ checking compared to YELLOW +
+ +
+
+
+ + more relaxed collision... + + + + + +
+
+
+ + Even if the vehicle passed the stopline +
+ but is expected to stop with more than a +
+ specific margin with ego path under the +
+ deceleration, ignore it +
+
+ Only check collision with vehicles +
+ inside intersection +
+ +
+
+
+ + Even if the vehicle passed the stopline... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg new file mode 100644 index 0000000000000..e0078540ba407 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg @@ -0,0 +1,750 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + w/ traffic light, right +
+
+ (Left-hand traffic) + + + +
+ + +
+
+
+ + w/ traffic light, right... + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention lane +
+
+
+ + attention lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + T-shape junction w/o traffic light +
+
+ Right-hand traffic +
+ +
+
+
+ + T-shape junction w/o traffic light... + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 8d9beb34d05ee..83e218185a5ad 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -196,6 +196,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.yield_stuck_detect_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, + 0.34509, 0.6588235), + &debug_marker_array); + } + if (debug_data_.ego_lane) { appendMarkerArray( createLaneletPolygonsMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 0d483b501d1ee..e036fac13fd3b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1056,29 +1056,33 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected && stuck_stopline_idx_opt) { - auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); + if (stuck_detected) { if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { - if ( - default_stopline_idx_opt && - fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) { - stuck_stopline_idx = default_stopline_idx_opt.value(); - } } else { - return IntersectionModule::StuckStop{ - closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt}; + std::optional stopline_idx = std::nullopt; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = default_stopline_idx_opt.value(); + } else if ( + first_attention_stopline_idx_opt && + fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return IntersectionModule::StuckStop{ + closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + } } } - // yield stuck vehicle detection is viable even if attention area is empty - // so this needs to be checked before attention area validation - const bool yield_stuck_detected = checkYieldStuckVehicle( - planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); - if (yield_stuck_detected && stuck_stopline_idx_opt) { - const auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx}; - } - // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; @@ -1124,6 +1128,32 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + const bool yield_stuck_detected = checkYieldStuckVehicle( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); + if (yield_stuck_detected) { + std::optional stopline_idx = std::nullopt; + const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; + const bool is_before_first_attention_stopline = + fromEgoDist(first_attention_stopline_idx) >= 0.0; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (is_before_default_stopline) { + stopline_idx = default_stopline_idx; + } else if (is_before_first_attention_stopline) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()}; + } + } + const double is_amber_or_red = (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); @@ -1376,13 +1406,10 @@ bool IntersectionModule::checkStuckVehicle( } bool IntersectionModule::checkYieldStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area) + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) { - if (!first_attention_area) { - return false; - } - const bool yield_stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || @@ -1392,13 +1419,9 @@ bool IntersectionModule::checkYieldStuckVehicle( return false; } - const auto & objects_ptr = planner_data->predicted_objects; - - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - return util::checkYieldStuckVehicleInIntersection( - objects_ptr, ego_poly, first_attention_area.value(), + target_objects, interpolated_path_info, attention_lanelets, turn_direction_, + planner_data_->vehicle_info_.vehicle_width_m, planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, planner_param_.yield_stuck.distance_threshold, &debug_data_); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 4c33c0960afc3..7366bdc1bc0e6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -336,9 +336,9 @@ class IntersectionModule : public SceneModuleInterface const util::PathLanelets & path_lanelets); bool checkYieldStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area); + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets); util::TargetObjects generateTargetObjects( const util::IntersectionLanelets & intersection_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 1c7e366347fec..e491d2ce7c5ce 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -298,7 +299,7 @@ std::optional generateIntersectionStopLines( const auto path_footprint = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // TODO(Mamoru Sobue): maybe consideration of braking dist is necessary + // NOTE: maybe consideration of braking dist is necessary first_footprint_attention_centerline_ip_opt = i; break; } @@ -1179,40 +1180,145 @@ bool checkStuckVehicleInIntersection( return false; } -bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, - const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) +static lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) { - const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); - Polygon2d first_attention_area_poly; - for (const auto & p : first_attention_area_2d) { - first_attention_area_poly.outer().emplace_back(p.x(), p.y()); - } - - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; // not stop vehicle + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; } + accumulated_length += length; + } - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} - // check if the object is too close to the ego path - if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { +static lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +bool checkYieldStuckVehicleInIntersection( + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, + const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + DebugData * debug_data) +{ + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { continue; } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - // check if the footprint is in the stuck detect area - const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); - if (is_in_stuck_area && debug_data) { - debug_data->yield_stuck_targets.objects.push_back(object); - return true; + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data->yield_stuck_targets.objects.push_back(object.object); + return true; + } } } return false; @@ -1527,6 +1633,7 @@ lanelet::ConstLanelet generatePathLanelet( const double yaw = tf2::getYaw(p.orientation); const double x = p.position.x; const double y = p.position.y; + // NOTE: maybe this is opposite const double left_x = x + width / 2 * std::sin(yaw); const double left_y = y - width / 2 * std::cos(yaw); const double right_x = x - width / 2 * std::sin(yaw); diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 8d0e673fc931e..33e511d911b82 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -131,9 +131,10 @@ bool checkStuckVehicleInIntersection( DebugData * debug_data); bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, - const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, + const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data); Polygon2d generateStuckVehicleDetectAreaPolygon( diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 3c7ba3041b0bd..d05031bf19436 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -45,6 +45,7 @@ struct DebugData std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 47d4985ae643e..e049d02ffe9b5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -230,6 +230,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( } const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); if (obj_v > planner_param_.stuck_vehicle_vel_thr) { + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::GREEN); continue; // not stop vehicle } // check if the footprint is in the stuck detect area @@ -237,6 +239,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); for (const auto & p : obj_footprint.outer()) { geometry_msgs::msg::Point point; point.x = p.x(); diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 69ad30e32dfdc..9f580149be7ae 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -123,7 +123,7 @@ bool isCollisionFree( return true; } -boost::optional generateOcclusionPolygon( +std::optional generateOcclusionPolygon( const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, const Point2d & max_theta_pos, const double ray_max_length = 100.0) { @@ -157,7 +157,7 @@ boost::optional generateOcclusionPolygon( occlusion_poly.outer().emplace_back(max_intersections.front()); } //! case outside detection area - if (occlusion_poly.outer().size() == 2) return boost::none; + if (occlusion_poly.outer().size() == 2) return std::nullopt; boost::geometry::correct(occlusion_poly); Polygon2d hull_poly; boost::geometry::convex_hull(occlusion_poly, hull_poly); @@ -200,7 +200,7 @@ std::pair calcEdgePoint(const Polygon2d & foot_print, const Poin return std::make_pair(min_idx, max_idx); } -boost::optional generateOccupiedPolygon( +std::optional generateOccupiedPolygon( const Polygon2d & occupancy_poly, const Polygon2d & foot_print, const Point & position) { Point2d origin = {position.x, position.y}; @@ -275,9 +275,9 @@ void generateOccupiedImage( for (const auto & foot_print : moving_vehicle_foot_prints) { // calculate occlusion polygon from moving vehicle const auto polys = generateOccupiedPolygon(occupancy_poly, foot_print, scan_origin); - if (polys == boost::none) continue; + if (polys == std::nullopt) continue; // transform to cv point and stuck it to cv polygon - for (const auto & p : polys.get().outer()) { + for (const auto & p : polys.value().outer()) { const Point transformed_geom_pt = transformFromMap2Grid(geom_tf_map2grid, p); cv_polygon.emplace_back( toCVPoint(transformed_geom_pt, width, height, occupancy_grid.info.resolution)); diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 849e16eb0834c..c9dbe13474b06 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -99,7 +99,7 @@ void findOcclusionSpots( bool isCollisionFree( const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2, const double radius); -boost::optional generateOccupiedPolygon( +std::optional generateOccupiedPolygon( const Polygon2d & occupancy_poly, const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, const Point & position); //!< @brief generate occupied polygon from foot print diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index fa8be96c02859..47e643241740d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -406,11 +406,11 @@ bool generatePossibleCollisionsFromGridMap( const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); - if (pc == boost::none) continue; - const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance); + if (pc) continue; + const double lateral_distance = std::abs(pc.value().arc_lane_dist_at_collision.distance); if (lateral_distance > distance_lower_bound) continue; distance_lower_bound = lateral_distance; - possible_collisions.emplace_back(pc.get()); + possible_collisions.emplace_back(pc.value()); } return !possible_collisions.empty(); } @@ -423,7 +423,7 @@ bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d return false; } -boost::optional generateOneNotableCollisionFromOcclusionSpot( +std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data) @@ -473,7 +473,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS has_collision = true; } if (has_collision) return candidate; - return boost::none; + return std::nullopt; } } // namespace occlusion_spot_utils diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index cc07461864c16..ec58e38dec45a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -30,8 +30,6 @@ #include #include -#include - #include #include #include @@ -67,7 +65,7 @@ using lanelet::ConstLineString2d; using lanelet::LaneletMapPtr; using lanelet::geometry::fromArcCoordinates; using lanelet::geometry::toArcCoordinates; -using DetectionAreaIdx = boost::optional>; +using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; namespace occlusion_spot_utils @@ -240,7 +238,7 @@ void calcSlowDownPointsForPossibleCollision( const int closest_idx, const PathWithLaneId & path, const double offset, std::vector & possible_collisions); //!< @brief convert a set of occlusion spots found on detection_area slice -boost::optional generateOneNotableCollisionFromOcclusionSpot( +std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 7c0036b7812fa..5da8df9e70c35 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -147,6 +147,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } planner_manager_.launchScenePlugin(*this, name); } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index ced2e267cc025..fcde16d8a871c 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,8 @@ namespace behavior_velocity_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; +using objects_of_interest_marker_interface::ColorName; +using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; using tier4_debug_msgs::msg::Float64Stamped; @@ -55,6 +58,19 @@ using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; using unique_identifier_msgs::msg::UUID; +struct ObjectOfInterest +{ + geometry_msgs::msg::Pose pose; + autoware_auto_perception_msgs::msg::Shape shape; + ColorName color; + ObjectOfInterest( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + : pose(pose), shape(shape), color(color_name) + { + } +}; + class SceneModuleInterface { public: @@ -94,6 +110,8 @@ class SceneModuleInterface void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } + std::vector getObjectsOfInterestData() const { return objects_of_interest_; } + void clearObjectsOfInterestData() { objects_of_interest_.clear(); } protected: const int64_t module_id_; @@ -107,6 +125,7 @@ class SceneModuleInterface std::optional infrastructure_command_; std::optional first_stop_path_point_index_; VelocityFactorInterface velocity_factor_; + std::vector objects_of_interest_; void setSafe(const bool safe) { @@ -118,6 +137,13 @@ class SceneModuleInterface void setDistance(const double distance) { distance_ = distance; } void syncActivation() { setActivation(isSafe()); } + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + { + objects_of_interest_.emplace_back(pose, shape, color_name); + } + size_t findEgoSegmentIndex( const std::vector & points) const; }; @@ -200,6 +226,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface RTCInterface rtc_interface_; std::unordered_map map_uuid_; + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + virtual void sendRTC(const Time & stamp); virtual void setActivation(); @@ -220,6 +248,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void removeUUID(const int64_t & module_id); + void publishObjectsOfInterestMarker(); + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 187720ff92a15..784b7cabfe9ad 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -32,6 +32,7 @@ motion_utils motion_velocity_smoother nav_msgs + objects_of_interest_marker_interface pcl_conversions rclcpp rclcpp_components diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 49a52fcd60df2..362493005eb17 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -174,7 +174,7 @@ void SceneModuleManagerInterface::deleteExpiredModules( void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); scene_modules_.insert(scene_module); @@ -183,7 +183,7 @@ void SceneModuleManagerInterface::registerModule( void SceneModuleManagerInterface::unregisterModule( const std::shared_ptr & scene_module) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "unregister task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.erase(scene_module->getModuleId()); @@ -192,7 +192,9 @@ void SceneModuleManagerInterface::unregisterModule( SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) { } @@ -202,6 +204,7 @@ void SceneModuleManagerInterfaceWithRTC::plan( setActivation(); modifyPathVelocity(path); sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); } void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) @@ -244,6 +247,19 @@ void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) } } +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index ea63462b32d84..5534228c1b86f 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,6 +3,7 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet: specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index 7bd27fca3407c..809579b383461 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -20,6 +20,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + behavior_velocity_crosswalk_module behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 91656d542ea8e..b28725a92628e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -35,14 +35,17 @@ class DebugValues public: enum class TYPE { CALCULATION_TIME = 0, - CALCULATION_TIME_COLLISION_CHECK = 1, - LATERAL_DIST = 2, - LONGITUDINAL_DIST_OBSTACLE = 3, - LONGITUDINAL_DIST_COLLISION = 4, - COLLISION_POS_FROM_EGO_FRONT = 5, - STOP_DISTANCE = 6, - NUM_OBSTACLES = 7, - LATERAL_PASS_DIST = 8, + CALCULATION_TIME_PATH_PROCESSING = 1, + CALCULATION_TIME_OBSTACLE_CREATION = 2, + CALCULATION_TIME_COLLISION_CHECK = 3, + CALCULATION_TIME_PATH_PLANNING = 4, + LATERAL_DIST = 5, + LONGITUDINAL_DIST_OBSTACLE = 6, + LONGITUDINAL_DIST_COLLISION = 7, + COLLISION_POS_FROM_EGO_FRONT = 8, + STOP_DISTANCE = 9, + NUM_OBSTACLES = 10, + LATERAL_PASS_DIST = 11, SIZE, // this is the number of enum elements }; diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 09c87ed81cf38..3ba9bf8bf52e6 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) auto & p = planner_param_.run_out; p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk"); p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin"); diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 2fa8cf241ee74..92516e7b4424b 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -14,6 +14,7 @@ #include "scene.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" #include @@ -23,6 +24,10 @@ #include #include +#include + +#include +#include #include #include @@ -60,7 +65,7 @@ bool RunOutModule::modifyPathVelocity( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { // timer starts - const auto t1_modify_path = std::chrono::system_clock::now(); + const auto t_start = std::chrono::system_clock::now(); // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; @@ -70,20 +75,27 @@ bool RunOutModule::modifyPathVelocity( // set height of debug data debug_ptr_->setHeight(current_pose.position.z); - // smooth velocity of the path to calculate time to collision accurately - PathWithLaneId smoothed_path; - if (!smoothPath(*path, smoothed_path, planner_data_)) { - return true; - } - // extend path to consider obstacles after the goal - const auto extended_smoothed_path = - run_out_utils::extendPath(smoothed_path, planner_param_.vehicle_param.base_to_front); + const auto extended_path = + run_out_utils::extendPath(*path, planner_param_.vehicle_param.base_to_front); // trim path ahead of the base_link to make calculation easier const double trim_distance = planner_param_.run_out.detection_distance; - const auto trim_smoothed_path = - run_out_utils::trimPathFromSelfPose(extended_smoothed_path, current_pose, trim_distance); + const auto trim_path = + run_out_utils::trimPathFromSelfPose(extended_path, current_pose, trim_distance); + + // smooth velocity of the path to calculate time to collision accurately + PathWithLaneId extended_smoothed_path; + if (!smoothPath(trim_path, extended_smoothed_path, planner_data_)) { + return true; + } + + // record time for path processing + const auto t_path_processing = std::chrono::system_clock::now(); + const auto elapsed_path_processing = + std::chrono::duration_cast(t_path_processing - t_start); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_PATH_PROCESSING, elapsed_path_processing.count() / 1000.0); // create abstracted dynamic obstacles from objects or points dynamic_obstacle_creator_->setData(*planner_data_, planner_param_, *path, extended_smoothed_path); @@ -92,18 +104,30 @@ bool RunOutModule::modifyPathVelocity( // extract obstacles using lanelet information const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, trim_smoothed_path, current_pose); + excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); - // timer starts - const auto t1_collision_check = std::chrono::system_clock::now(); + // record time for obstacle creation + const auto t_obstacle_creation = std::chrono::system_clock::now(); + const auto elapsed_obstacle_creation = + std::chrono::duration_cast(t_obstacle_creation - t_path_processing); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_OBSTACLE_CREATION, + elapsed_obstacle_creation.count() / 1000.0); // detect collision with dynamic obstacles using velocity planning of ego - const auto dynamic_obstacle = detectCollision(partition_excluded_obstacles, trim_smoothed_path); - - // timer ends - const auto t2_collision_check = std::chrono::system_clock::now(); + const auto crosswalk_lanelets = planner_param_.run_out.suppress_on_crosswalk + ? getCrosswalksOnPath( + planner_data_->current_odometry->pose, *path, + planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->route_handler_->getOverallGraphPtr()) + : std::vector>(); + const auto dynamic_obstacle = + detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets); + + // record time for collision check + const auto t_collision_check = std::chrono::system_clock::now(); const auto elapsed_collision_check = - std::chrono::duration_cast(t2_collision_check - t1_collision_check); + std::chrono::duration_cast(t_collision_check - t_obstacle_creation); debug_ptr_->setDebugValues( DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); @@ -112,7 +136,7 @@ bool RunOutModule::modifyPathVelocity( // after a certain amount of time has elapsed since the ego stopped, // approach the obstacle with slow velocity insertVelocityForState( - dynamic_obstacle, *planner_data_, planner_param_, trim_smoothed_path, *path); + dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path); } else { // just insert zero velocity for stopping insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); @@ -124,20 +148,26 @@ bool RunOutModule::modifyPathVelocity( } publishDebugValue( - trim_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); - // timer ends - const auto t2_modify_path = std::chrono::system_clock::now(); - const auto elapsed_modify_path = - std::chrono::duration_cast(t2_modify_path - t1_modify_path); + // record time for collision check + const auto t_path_planning = std::chrono::system_clock::now(); + const auto elapsed_path_planning = + std::chrono::duration_cast(t_path_planning - t_collision_check); debug_ptr_->setDebugValues( - DebugValues::TYPE::CALCULATION_TIME, elapsed_modify_path.count() / 1000.0); + DebugValues::TYPE::CALCULATION_TIME_PATH_PLANNING, elapsed_path_planning.count() / 1000.0); + + // record time for the function + const auto t_end = std::chrono::system_clock::now(); + const auto elapsed_all = std::chrono::duration_cast(t_end - t_start); + debug_ptr_->setDebugValues(DebugValues::TYPE::CALCULATION_TIME, elapsed_all.count() / 1000.0); return true; } std::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const std::vector> & crosswalk_lanelets) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -171,7 +201,7 @@ std::optional RunOutModule::detectCollision( debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0); auto obstacles_collision = - checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time); + checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets); if (obstacles_collision.empty()) { continue; } @@ -291,7 +321,8 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time) const + std::vector poly, const float travel_time, + const std::vector> & crosswalk_lanelets) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -320,8 +351,8 @@ std::vector RunOutModule::checkCollisionWithObstacles( *predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel}; std::vector collision_points; - const bool collision_detected = - checkCollisionWithShape(bg_poly_vehicle, pose_with_range, obstacle.shape, collision_points); + const bool collision_detected = checkCollisionWithShape( + bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points); if (!collision_detected) { continue; @@ -380,6 +411,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( bool RunOutModule::checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + const std::vector> & crosswalk_lanelets, std::vector & collision_points) const { bool collision_detected = false; @@ -402,6 +434,23 @@ bool RunOutModule::checkCollisionWithShape( break; } + if (!collision_points.empty()) { + for (const auto & crosswalk : crosswalk_lanelets) { + const auto poly = crosswalk.second.polygon2d().basicPolygon(); + for (auto it = collision_points.begin(); it != collision_points.end();) { + if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) { + it = collision_points.erase(it); + } else { + ++it; + } + } + if (collision_points.empty()) { + break; + } + } + collision_detected = !collision_points.empty(); + } + return collision_detected; } diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index b1c49189267d4..def90f036c440 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -67,7 +68,8 @@ class RunOutModule : public SceneModuleInterface Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; std::optional detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const std::vector> & crosswalk_lanelets); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -77,7 +79,8 @@ class RunOutModule : public SceneModuleInterface std::vector checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time) const; + std::vector poly, const float travel_time, + const std::vector> & crosswalk_lanelets) const; std::optional findNearestCollisionObstacle( const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, @@ -89,6 +92,7 @@ class RunOutModule : public SceneModuleInterface bool checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + const std::vector> & crosswalk_lanelets, std::vector & collision_points) const; bool checkCollisionWithCylinder( diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 6afe451f72f73..5524c0f76049d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -54,6 +54,7 @@ struct RunOutParam { std::string detection_method; bool use_partition_lanelet; + bool suppress_on_crosswalk; bool specify_decel_jerk; double stop_margin; double passing_margin; diff --git a/planning/behavior_velocity_traffic_light_module/README.md b/planning/behavior_velocity_traffic_light_module/README.md index 866ab87070369..e85a171495e38 100644 --- a/planning/behavior_velocity_traffic_light_module/README.md +++ b/planning/behavior_velocity_traffic_light_module/README.md @@ -19,7 +19,13 @@ This module is activated when there is traffic light in ego lane. 1. Obtains a traffic light mapped to the route and a stop line correspond to the traffic light from a map information. -2. Uses the highest reliability one of the traffic light recognition result and if the color of that was red, generates a stop point. + - If a corresponding traffic light signal have never been found, it treats as a signal to pass. + + - If a corresponding traffic light signal is found but timed out, it treats as a signal to stop. + +2. Uses the highest reliability one of the traffic light recognition result and if the color of that was not green or corresponding arrow signal, generates a stop point. + + - If an elapsed time to receive stop signal is less than `stop_time_hysteresis`, it treats as a signal to pass. This feature is to prevent chattering. 3. When vehicle current velocity is @@ -63,12 +69,13 @@ This module is activated when there is traffic light in ego lane. #### Module Parameters -| Parameter | Type | Description | -| -------------------- | ------ | ----------------------------------------------- | -| `stop_margin` | double | [m] margin before stop point | -| `tl_state_timeout` | double | [s] time out for detected traffic light result. | -| `yellow_lamp_period` | double | [s] time for yellow lamp | -| `enable_pass_judge` | bool | [-] whether to use pass judge | +| Parameter | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------- | +| `stop_margin` | double | [m] margin before stop point | +| `tl_state_timeout` | double | [s] time out for detected traffic light result. | +| `stop_time_hysteresis` | double | [s] time threshold to decide stop planning for chattering prevention | +| `yellow_lamp_period` | double | [s] time for yellow lamp | +| `enable_pass_judge` | bool | [-] whether to use pass judge | #### Flowchart @@ -91,7 +98,7 @@ if (state is APPROACH) then (yes) :change state to GO_OUT; stop elseif (no stop signal) then (yes) - :change previous state to STOP; + :change previous state to PASS; stop elseif (not pass through) then (yes) :insert stop pose; diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index a837ae1b46b9b..23746a61b6043 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -3,6 +3,7 @@ traffic_light: stop_margin: 0.0 tl_state_timeout: 1.0 + stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 enable_pass_judge: true enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index a23edefff52ab..908627540bdcc 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -38,6 +38,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); planner_param_.tl_state_timeout = getOrDeclareParameter(node, ns + ".tl_state_timeout"); + planner_param_.stop_time_hysteresis = + getOrDeclareParameter(node, ns + ".stop_time_hysteresis"); planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 90a547adeb930..a74ff8c0cb5e8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -222,13 +222,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (signed_arc_length_to_stop_point < signed_deadline_length) { RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); state_ = State::GO_OUT; + stop_signal_received_time_ptr_.reset(); return true; } first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal()); + const bool is_stop_signal = isStopSignal(); + + // Update stop signal received time + if (is_stop_signal) { + if (!stop_signal_received_time_ptr_) { + stop_signal_received_time_ptr_ = std::make_unique