From 5d4727e236ee4c0644532c9f362cfc740237679e Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 30 Mar 2023 10:17:21 +0900 Subject: [PATCH 01/19] chore: sync files (#3214) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/workflows/github-release.yaml | 2 +- .github/workflows/pre-commit.yaml | 5 ----- .pre-commit-config-optional.yaml | 2 +- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 33677fc1b762c..95ebb8725f62b 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -27,7 +27,7 @@ jobs: fi echo "ref-name=$REF_NAME" >> $GITHUB_OUTPUT - echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT" + echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index bda722c87ef09..b231dbda87938 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -20,11 +20,6 @@ jobs: with: ref: ${{ github.event.pull_request.head.ref }} - - name: Set git config - uses: autowarefoundation/autoware-github-actions/set-git-config@v1 - with: - token: ${{ steps.generate-token.outputs.token }} - - name: Run pre-commit uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index caa92d0812ab0..e0019e10d5210 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.11.0 + rev: v3.10.3 hooks: - id: markdown-link-check args: [--config=.markdown-link-check.json] From 487c6787028b7456c6cb3c6d220c177994d54f63 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 30 Mar 2023 11:25:06 +0900 Subject: [PATCH 02/19] fix(behavior_path_planner): fix lane change distance check (#3211) Signed-off-by: yutaka --- .../util/lane_change/util.hpp | 2 +- .../src/util/lane_change/util.cpp | 32 ++++++++++--------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index d8def1bf870c3..984bd8e6d4dd2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -55,7 +55,7 @@ bool isPathInLanelets( const lanelet::ConstLanelets & target_lanelets); double calcLaneChangingDistance( - const double lane_changing_velocity, const double shift_length, const double min_total_lc_len, + const double lane_changing_velocity, const double shift_length, const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param); std::optional constructCandidatePath( diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index c3b74ed68730d..c0d8c07c2581f 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -310,18 +310,12 @@ std::pair getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); - const auto end_of_lane_dist = std::invoke([&]() { - const auto required_dist = util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); - if (is_goal_in_route) { - return util::getSignedDistance(pose, route_handler.getGoalPose(), original_lanelets) - - required_dist; - } - return util::getDistanceToEndOfLane(pose, original_lanelets) - required_dist; - }); - const auto required_total_min_distance = util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); + const auto dist_to_end_of_current_lane = + util::getDistanceToEndOfLane(pose, original_lanelets) - required_total_min_distance; + [[maybe_unused]] const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose); const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); @@ -380,14 +374,22 @@ std::pair getLaneChangePaths( // we assume constant speed during lane change const auto lane_changing_speed = std::max(prepare_speed, minimum_lane_change_velocity); const auto lane_changing_distance = calcLaneChangingDistance( - lane_changing_speed, estimated_shift_length, end_of_lane_dist, common_parameter, parameter); + lane_changing_speed, estimated_shift_length, common_parameter, parameter); + + if (lane_changing_distance + prepare_distance > dist_to_end_of_current_lane) { + // total lane changing distance it too long + continue; + } if (is_goal_in_route) { const double s_start = lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length; const double s_goal = lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; - if (s_start + lane_changing_distance + parameter.lane_change_finish_judge_buffer > s_goal) { + if ( + s_start + lane_changing_distance + parameter.lane_change_finish_judge_buffer + + required_total_min_distance > + s_goal) { continue; } } @@ -727,15 +729,15 @@ PathWithLaneId getPrepareSegment( } double calcLaneChangingDistance( - const double lane_changing_speed, const double shift_length, const double min_total_lc_len, + const double lane_changing_speed, const double shift_length, const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param) { const auto required_time = PathShifter::calcShiftTimeFromJerk( shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); - const auto expected_dist = lane_changing_speed * required_time; - const auto lane_changing_distance = - (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length; + const double & min_lane_change_length = com_param.minimum_lane_change_length; + const double lane_changing_distance = + std::max(lane_changing_speed * required_time, min_lane_change_length); RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") From ce94303a430060c7cfca738ae680ddcaf1a8f485 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 30 Mar 2023 23:18:18 +0900 Subject: [PATCH 03/19] fix(motion_utils): change lane_id resampling method (#3195) * fix(motion_utils): change lane_id resampling method Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- common/motion_utils/src/resample/resample.cpp | 75 +++++++++++++++++-- 1 file changed, 68 insertions(+), 7 deletions(-) diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index e9b6dcbc34cce..107a77393d5a2 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -179,8 +179,38 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { + auto resampling_arclength = resampled_arclength; + + // Add resampling_arclength to insert input points which have multiple lane_ids + for (size_t i = 0; i < input_path.points.size(); ++i) { + if (input_path.points.at(i).lane_ids.size() < 2) { + continue; + } + + const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i); + for (size_t i = 1; i < resampling_arclength.size(); ++i) { + if ( + resampling_arclength.at(i - 1) <= distance_to_resampling_point && + distance_to_resampling_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + std::fabs(distance_to_resampling_point - resampling_arclength.at(i - 1)); + const double dist_to_following_point = + std::fabs(resampling_arclength.at(i) - distance_to_resampling_point); + if (dist_to_prev_point < motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = distance_to_resampling_point; + } else if (dist_to_following_point < motion_utils::overlap_threshold) { + resampling_arclength.at(i) = distance_to_resampling_point; + } else { + resampling_arclength.insert( + resampling_arclength.begin() + i, distance_to_resampling_point); + } + break; + } + } + } + // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { + if (!resample_utils::validate_arguments(input_path.points, resampling_arclength)) { return input_path; } @@ -233,7 +263,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( lane_ids.push_back(input_path.points.at(i).lane_ids); } - if (input_arclength.back() < resampled_arclength.back()) { + if (input_arclength.back() < resampling_arclength.back()) { std::cerr << "[motion_utils]: resampled path length is longer than input path length" << std::endl; return input_path; @@ -241,25 +271,56 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return interpolation::lerp(input_arclength, input, resampling_arclength); }; auto closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); const auto zoh = [&](const auto & input) { return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); const auto interpolated_is_final = zoh(is_final); - const auto interpolated_lane_ids = zoh(lane_ids); - if (interpolated_pose.size() != resampled_arclength.size()) { + // interpolate lane_ids + std::vector> interpolated_lane_ids; + interpolated_lane_ids.resize(resampling_arclength.size()); + constexpr double epsilon = 1e-6; + for (size_t i = 0; i < resampling_arclength.size(); ++i) { + const size_t seg_idx = std::min(closest_segment_indices.at(i), input_path.points.size() - 2); + const auto & prev_lane_ids = input_path.points.at(seg_idx).lane_ids; + const auto & next_lane_ids = input_path.points.at(seg_idx + 1).lane_ids; + + if (std::abs(input_arclength.at(seg_idx) - resampling_arclength.at(i)) <= epsilon) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); + } else if (std::abs(input_arclength.at(seg_idx + 1) - resampling_arclength.at(i)) <= epsilon) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), next_lane_ids.begin(), next_lane_ids.end()); + } else { + // extract lane_ids those prev_lane_ids and next_lane_ids have in common + for (const auto target_lane_id : prev_lane_ids) { + if ( + std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != + next_lane_ids.end()) { + interpolated_lane_ids.at(i).push_back(target_lane_id); + } + } + // If there are no common lane_ids, the prev_lane_ids is assigned. + if (interpolated_lane_ids.at(i).empty()) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); + } + } + } + + if (interpolated_pose.size() != resampling_arclength.size()) { std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" << std::endl; return input_path; From 33092b07b61475c67058f83a9c90425f206ad2cb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 30 Mar 2023 23:18:29 +0900 Subject: [PATCH 04/19] fix(behavior_path_planner): use cropPoints (#3196) Signed-off-by: Takayuki Murooka --- .../src/behavior_path_planner_node.cpp | 10 ++++++++-- planning/behavior_path_planner/src/path_utilities.cpp | 2 ++ .../lane_following/lane_following_module.cpp | 8 ++++++-- 3 files changed, 16 insertions(+), 4 deletions(-) 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 1154816c90af3..61233eaff71be 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1106,8 +1106,14 @@ void BehaviorPathPlannerNode::run() // publish drivable bounds publish_bounds(*path); - const size_t target_idx = planner_data_->findEgoIndex(path->points); - util::clipPathLength(*path, target_idx, planner_data_->parameters); + // NOTE: In order to keep backward_path_length at least, resampling interval is added to the + // backward. + const auto current_pose = planner_data_->self_odometry->pose.pose; + const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points); + path->points = motion_utils::cropPoints( + path->points, current_pose.position, current_seg_idx, + planner_data_->parameters.forward_path_length, + planner_data_->parameters.backward_path_length + planner_data_->parameters.input_path_interval); if (!path->points.empty()) { path_publisher_->publish(*path); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 3f01b7596bb80..2fc35fef0d491 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -170,6 +170,7 @@ size_t getIdxByArclength( } } +// TODO(murooka) This function should be replaced with motion_utils::cropPoints void clipPathLength( PathWithLaneId & path, const size_t target_idx, const double forward, const double backward) { @@ -186,6 +187,7 @@ void clipPathLength( path.points = clipped_points; } +// TODO(murooka) This function should be replaced with motion_utils::cropPoints void clipPathLength( PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params) { diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 4cdca4dd09e53..56508b1884611 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -134,9 +134,13 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const p.forward_path_length, p); // clip backward length + // NOTE: In order to keep backward_path_length at least, resampling interval is added to the + // backward. const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(reference_path.points); - util::clipPathLength( - reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length); + reference_path.points = motion_utils::cropPoints( + reference_path.points, current_pose.position, current_seg_idx, p.forward_path_length, + p.backward_path_length + p.input_path_interval); + const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); const auto drivable_lanes = util::generateDrivableLanes(drivable_lanelets); From e8c1d73274e6cf7d5e51bca1338fff6c95ae219a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 31 Mar 2023 09:33:54 +0900 Subject: [PATCH 05/19] feat(behavior_path_planner): update behavior param file (#3220) * feat(behavior_path_planner): add new config file for manger Signed-off-by: satoshi-ota * feat(launch): add config path Signed-off-by: satoshi-ota * fix(behavior_path_planner): add missing param file Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_planning.launch.py | 3 + .../config/drivable_area_expansion.param.yaml | 58 +++++++++++++++++++ .../config/scene_module_manager.param.yaml | 52 +++++++++++++++++ 3 files changed, 113 insertions(+) create mode 100644 planning/behavior_path_planner/config/drivable_area_expansion.param.yaml create mode 100644 planning/behavior_path_planner/config/scene_module_manager.param.yaml diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 4616590facb06..fcac604feb1dc 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -54,6 +54,8 @@ def launch_setup(context, *args, **kwargs): pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("scene_module_manager_param_path").perform(context), "r") as f: + scene_module_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f: behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -88,6 +90,7 @@ def launch_setup(context, *args, **kwargs): pull_over_param, pull_out_param, drivable_area_expansion_param, + scene_module_manager_param, behavior_path_planner_param, vehicle_param, { diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml new file mode 100644 index 0000000000000..902a93ce6d540 --- /dev/null +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + # Static expansion + avoidance: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_change: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_following: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_out: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_over: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + side_shift: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + + # Dynamic expansion by projecting the ego footprint along the path + dynamic_expansion: + enabled: false + ego: + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the ego footprint + rear: 0.5 # [m] extra length to add to the rear of the ego footprint + left: 0.5 # [m] extra length to add to the left of the ego footprint + right: 0.5 # [m] extra length to add to the rear of the ego footprint + dynamic_objects: + avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the dynamic object footprint + rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + left: 0.5 # [m] extra length to add to the left of the dynamic object footprint + right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + expansion: + method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. + # 'lanelet': add lanelets overlapped by the ego footprints + # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area + max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + avoid_linestring: + types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area + - road_border + distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid + compensate: + enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction + extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml new file mode 100644 index 0000000000000..098af56ede940 --- /dev/null +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -0,0 +1,52 @@ +# USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE. +# https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt +# NOTE: The smaller the priority number is, the higher the module priority is. +/**: + ros__parameters: + ext_request_lane_change_left: + enable_module: false + enable_simultaneous_execution: false + priority: 6 + max_module_size: 1 + + ext_request_lane_change_right: + enable_module: false + enable_simultaneous_execution: false + priority: 6 + max_module_size: 1 + + lane_change_left: + enable_module: true + enable_simultaneous_execution: true + priority: 5 + max_module_size: 1 + + lane_change_right: + enable_module: true + enable_simultaneous_execution: false + priority: 4 + max_module_size: 1 + + pull_out: + enable_module: true + enable_simultaneous_execution: false + priority: 0 + max_module_size: 1 + + side_shift: + enable_module: true + enable_simultaneous_execution: false + priority: 2 + max_module_size: 1 + + pull_over: + enable_module: true + enable_simultaneous_execution: false + priority: 1 + max_module_size: 1 + + avoidance: + enable_module: true + enable_simultaneous_execution: false + priority: 3 + max_module_size: 1 From a9ff35d5f5874c541fed28043de10f14a69e231f Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 31 Mar 2023 10:22:27 +0900 Subject: [PATCH 06/19] refactor(behavior_path_planner): change variable names (#3225) --- .../util/lane_change/util.hpp | 9 ++-- .../src/util/lane_change/util.cpp | 52 +++++++++---------- 2 files changed, 30 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index 984bd8e6d4dd2..7836d05f971b0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -59,7 +59,7 @@ double calcLaneChangingDistance( const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param); std::optional constructCandidatePath( - const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const double acceleration, const double prepare_distance, const double prepare_duration, @@ -106,8 +106,9 @@ bool hasEnoughDistance( #endif ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, - const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path); + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path, + const double shift_length); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -125,7 +126,7 @@ PathWithLaneId getPrepareSegment( const Pose & current_pose, const double backward_path_length, const double prepare_distance, const double prepare_speed); -PathWithLaneId getLaneChangingSegment( +PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_distance, diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index c0d8c07c2581f..64e94c61c90df 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -161,7 +161,7 @@ bool isPathInLanelets( } std::optional constructCandidatePath( - const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const std::vector> & sorted_lane_ids, const double acceleration, @@ -216,7 +216,7 @@ std::optional constructCandidatePath( "prepare_distance: %f, lane_change: %f", prepare_distance, lane_change_distance); const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); - const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); + const PathPointWithLaneId & lane_changing_end_point = target_segment.points.front(); const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; const auto lane_change_end_idx = motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); @@ -240,8 +240,8 @@ std::optional constructCandidatePath( point.point.longitudinal_velocity_mps = std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); const auto nearest_idx = - motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); - point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; + motion_utils::findNearestIndex(target_segment.points, point.point.pose); + point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; } // check candidate path is in lanelet @@ -313,7 +313,7 @@ std::pair getLaneChangePaths( const auto required_total_min_distance = util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); - const auto dist_to_end_of_current_lane = + const auto dist_to_end_of_current_lanes = util::getDistanceToEndOfLane(pose, original_lanelets) - required_total_min_distance; [[maybe_unused]] const auto arc_position_from_current = @@ -368,15 +368,15 @@ std::pair getLaneChangePaths( // lane changing start pose is at the end of prepare segment const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto estimated_shift_length = + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); // we assume constant speed during lane change const auto lane_changing_speed = std::max(prepare_speed, minimum_lane_change_velocity); - const auto lane_changing_distance = calcLaneChangingDistance( - lane_changing_speed, estimated_shift_length, common_parameter, parameter); + const auto lane_changing_distance = + calcLaneChangingDistance(lane_changing_speed, shift_length, common_parameter, parameter); - if (lane_changing_distance + prepare_distance > dist_to_end_of_current_lane) { + if (lane_changing_distance + prepare_distance > dist_to_end_of_current_lanes) { // total lane changing distance it too long continue; } @@ -394,14 +394,14 @@ std::pair getLaneChangePaths( } } - const auto lane_changing_segment = getLaneChangingSegment( + const auto target_segment = getTargetSegment( route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, target_lane_length, lane_changing_distance, lane_changing_speed, required_total_min_distance); - if (lane_changing_segment.points.empty()) { + if (target_segment.points.empty()) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "lane changing segment is empty!! something wrong..."); + "target segment is empty!! something wrong..."); continue; } @@ -418,14 +418,13 @@ std::pair getLaneChangePaths( } const auto shift_line = getLaneChangingShiftLine( - prepare_segment, lane_changing_segment, target_lanelets, target_lane_reference_path); + prepare_segment, target_segment, target_lanelets, target_lane_reference_path, shift_length); const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( - prepare_segment, lane_changing_segment, target_lane_reference_path, shift_line, - original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_dist, lc_speed, - parameter); + prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, + target_lanelets, sorted_lane_ids, acceleration, lc_dist, lc_speed, parameter); if (!candidate_path) { continue; @@ -652,15 +651,15 @@ bool isLaneChangePathSafe( } ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, - const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path) + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path, + const double shift_length) { const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const Pose & lane_changing_end_pose = lane_changing_segment.points.front().point.pose; + const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; ShiftLine shift_line; - shift_line.end_shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; shift_line.end = lane_changing_end_pose; shift_line.start_idx = @@ -750,7 +749,7 @@ double calcLaneChangingDistance( return lane_changing_distance; } -PathWithLaneId getLaneChangingSegment( +PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_distance, @@ -778,17 +777,16 @@ PathWithLaneId getLaneChangingSegment( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") .get_child("util") - .get_child("getLaneChangingSegment"), + .get_child("getTargetSegment"), "start: %f, end: %f", s_start, s_end); - PathWithLaneId lane_changing_segment = - route_handler.getCenterLinePath(target_lanelets, s_start, s_end); - for (auto & point : lane_changing_segment.points) { + PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); + for (auto & point : target_segment.points) { point.point.longitudinal_velocity_mps = std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); } - return lane_changing_segment; + return target_segment; } PathWithLaneId getReferencePathFromTargetLane( From c5e6cb21e61d898b87166147f2e72a8d01034181 Mon Sep 17 00:00:00 2001 From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com> Date: Fri, 31 Mar 2023 11:08:06 +0900 Subject: [PATCH 07/19] refactor(trajectory_follower_node): delete default values (#3104) * delete param Signed-off-by: yamazakiTasuku * style(pre-commit): autofix --------- Signed-off-by: yamazakiTasuku Co-authored-by: yamazakiTasuku Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- control/trajectory_follower_node/src/controller_node.cpp | 6 +++--- .../src/simple_trajectory_follower.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index b9966263a5504..5ee9f7f4f1e71 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -35,7 +35,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control timeout_thr_sec_ = declare_parameter("timeout_thr_sec"); const auto lateral_controller_mode = - getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc")); + getLateralControllerMode(declare_parameter("lateral_controller_mode")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { lateral_controller_ = std::make_shared(*this); @@ -49,8 +49,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LateralController] invalid algorithm"); } - const auto longitudinal_controller_mode = getLongitudinalControllerMode( - declare_parameter("longitudinal_controller_mode", "pid")); + const auto longitudinal_controller_mode = + getLongitudinalControllerMode(declare_parameter("longitudinal_controller_mode")); switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index 0af901be16125..9d0160a8cc65f 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -36,9 +36,9 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o sub_trajectory_ = create_subscription( "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); - use_external_target_vel_ = declare_parameter("use_external_target_vel", false); - external_target_vel_ = declare_parameter("external_target_vel", 0.0); - lateral_deviation_ = declare_parameter("lateral_deviation", 0.0); + use_external_target_vel_ = declare_parameter("use_external_target_vel"); + external_target_vel_ = declare_parameter("external_target_vel"); + lateral_deviation_ = declare_parameter("lateral_deviation"); using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer( From 201ae00598864aa0a0de3815411269e1094eb2c4 Mon Sep 17 00:00:00 2001 From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com> Date: Fri, 31 Mar 2023 11:18:20 +0900 Subject: [PATCH 08/19] refactor(scenario_selector): delete default values (#3116) scenario_selector_delete_param Signed-off-by: yamazakiTasuku Co-authored-by: yamazakiTasuku --- .../scenario_selector_node/scenario_selector_node.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index a9e82f46161e6..8f20b6b655d3b 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -351,11 +351,11 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), current_scenario_(tier4_planning_msgs::msg::Scenario::EMPTY), - update_rate_(this->declare_parameter("update_rate", 10.0)), - th_max_message_delay_sec_(this->declare_parameter("th_max_message_delay_sec", 1.0)), - th_arrived_distance_m_(this->declare_parameter("th_arrived_distance_m", 1.0)), - th_stopped_time_sec_(this->declare_parameter("th_stopped_time_sec", 1.0)), - th_stopped_velocity_mps_(this->declare_parameter("th_stopped_velocity_mps", 0.01)), + update_rate_(this->declare_parameter("update_rate")), + th_max_message_delay_sec_(this->declare_parameter("th_max_message_delay_sec")), + th_arrived_distance_m_(this->declare_parameter("th_arrived_distance_m")), + th_stopped_time_sec_(this->declare_parameter("th_stopped_time_sec")), + th_stopped_velocity_mps_(this->declare_parameter("th_stopped_velocity_mps")), is_parking_completed_(false) { // Input From 62c5bee630da2d1e22c4a0e2e8a18dfb49b2a8f5 Mon Sep 17 00:00:00 2001 From: mackierx111 Date: Fri, 31 Mar 2023 11:30:33 +0900 Subject: [PATCH 09/19] feat(traffic_light_map_based_detector): change the vibration parameter of the Traffic Light Detector (#3189) Change the vibration parameter of the Traffic Light Detector. Improve the accuracy of yellow signal recognition in AWSIM. --- .../config/traffic_light_map_based_detector.param.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml index 38fc290464857..6acb255a79aa2 100644 --- a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml +++ b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - max_vibration_pitch: 0.01745329251 # -0.5 ~ 0.5 deg - max_vibration_yaw: 0.01745329251 # -0.5 ~ 0.5 deg - max_vibration_height: 0.1 # -0.05 ~ 0.05 m - max_vibration_width: 0.1 # -0.05 ~ 0.05 m - max_vibration_depth: 0.1 # -0.05 ~ 0.05 m + max_vibration_pitch: 0.00436332312 # -0.5 ~ 0.5 deg + max_vibration_yaw: 0.00436332312 # -0.5 ~ 0.5 deg + max_vibration_height: 0.01 # -0.05 ~ 0.05 m + max_vibration_width: 0.01 # -0.05 ~ 0.05 m + max_vibration_depth: 0.01 # -0.05 ~ 0.05 m From 9952f94e6c37c18c7e78c0c1178de171dbd01b8d Mon Sep 17 00:00:00 2001 From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com> Date: Fri, 31 Mar 2023 11:32:25 +0900 Subject: [PATCH 10/19] refactor(costmap_generator): delete default values (#3129) delete param Signed-off-by: yamazakiTasuku Co-authored-by: yamazakiTasuku --- .../costmap_generator_node.cpp | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 473e0875609ea..381b8b472adfa 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -161,26 +161,26 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) : Node("costmap_generator", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // Parameters - costmap_frame_ = this->declare_parameter("costmap_frame", "map"); - vehicle_frame_ = this->declare_parameter("vehicle_frame", "base_link"); - map_frame_ = this->declare_parameter("map_frame", "map"); - update_rate_ = this->declare_parameter("update_rate", 10.0); - activate_by_scenario_ = this->declare_parameter("activate_by_scenario", true); - grid_min_value_ = this->declare_parameter("grid_min_value", 0.0); - grid_max_value_ = this->declare_parameter("grid_max_value", 1.0); - grid_resolution_ = this->declare_parameter("grid_resolution", 0.2); - grid_length_x_ = this->declare_parameter("grid_length_x", 50); - grid_length_y_ = this->declare_parameter("grid_length_y", 30); - grid_position_x_ = this->declare_parameter("grid_position_x", 20); - grid_position_y_ = this->declare_parameter("grid_position_y", 0); - maximum_lidar_height_thres_ = this->declare_parameter("maximum_lidar_height_thres", 0.3); - minimum_lidar_height_thres_ = this->declare_parameter("minimum_lidar_height_thres", -2.2); - use_objects_ = this->declare_parameter("use_objects", true); - use_points_ = this->declare_parameter("use_points", true); - use_wayarea_ = this->declare_parameter("use_wayarea", true); - use_parkinglot_ = this->declare_parameter("use_parkinglot", true); - expand_polygon_size_ = this->declare_parameter("expand_polygon_size", 1.0); - size_of_expansion_kernel_ = this->declare_parameter("size_of_expansion_kernel", 9); + costmap_frame_ = this->declare_parameter("costmap_frame"); + vehicle_frame_ = this->declare_parameter("vehicle_frame"); + map_frame_ = this->declare_parameter("map_frame"); + update_rate_ = this->declare_parameter("update_rate"); + activate_by_scenario_ = this->declare_parameter("activate_by_scenario"); + grid_min_value_ = this->declare_parameter("grid_min_value"); + grid_max_value_ = this->declare_parameter("grid_max_value"); + grid_resolution_ = this->declare_parameter("grid_resolution"); + grid_length_x_ = this->declare_parameter("grid_length_x"); + grid_length_y_ = this->declare_parameter("grid_length_y"); + grid_position_x_ = this->declare_parameter("grid_position_x"); + grid_position_y_ = this->declare_parameter("grid_position_y"); + maximum_lidar_height_thres_ = this->declare_parameter("maximum_lidar_height_thres"); + minimum_lidar_height_thres_ = this->declare_parameter("minimum_lidar_height_thres"); + use_objects_ = this->declare_parameter("use_objects"); + use_points_ = this->declare_parameter("use_points"); + use_wayarea_ = this->declare_parameter("use_wayarea"); + use_parkinglot_ = this->declare_parameter("use_parkinglot"); + expand_polygon_size_ = this->declare_parameter("expand_polygon_size"); + size_of_expansion_kernel_ = this->declare_parameter("size_of_expansion_kernel"); // Wait for first tf // We want to do this before creating subscriptions From 1b0a352274cc47e1c0c35b7f6b9d46a47a5f0aba Mon Sep 17 00:00:00 2001 From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com> Date: Fri, 31 Mar 2023 11:44:43 +0900 Subject: [PATCH 11/19] refactor(rtc_auto_mode_manager): delete default values (#3117) delete_param Signed-off-by: yamazakiTasuku Co-authored-by: yamazakiTasuku --- planning/rtc_auto_mode_manager/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp index a37987c2efe39..6092d2741d909 100644 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ b/planning/rtc_auto_mode_manager/src/node.cpp @@ -23,9 +23,9 @@ RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_ : Node("rtc_auto_mode_manager_node", node_options) { const std::vector module_list = - declare_parameter("module_list", std::vector()); + declare_parameter>("module_list"); const std::vector default_enable_list = - declare_parameter("default_enable_list", std::vector()); + declare_parameter>("default_enable_list"); for (const auto & module_name : module_list) { const bool enabled = From 89e63321aecb1ee1a1493300b90f23f5d2888a35 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 31 Mar 2023 12:38:11 +0900 Subject: [PATCH 12/19] refactor(motion_utils): move decel dist calculation to utils (#3222) * refactor(motion_utils): move decel dist calculation to utils Signed-off-by: kosuke55 * add test Signed-off-by: kosuke55 * Update common/motion_utils/src/distance/distance.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * reafactor test Signed-off-by: kosuke55 * add anonymouse namespace Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- common/motion_utils/CMakeLists.txt | 1 + .../motion_utils/distance/distance.hpp | 34 +++ common/motion_utils/src/distance/distance.cpp | 267 ++++++++++++++++++ .../test/src/distance/test_distance.cpp | 116 ++++++++ .../avoidance/avoidance_module.hpp | 4 +- .../util/avoidance/util.hpp | 4 - .../avoidance/avoidance_module.cpp | 31 +- .../src/util/avoidance/util.cpp | 194 ------------- .../include/scene_module/run_out/utils.hpp | 51 +--- .../src/scene_module/run_out/scene.cpp | 2 +- .../src/scene_module/run_out/utils.cpp | 170 +---------- .../obstacle_stop_planner/planner_utils.hpp | 50 ---- .../src/planner_utils.cpp | 172 +---------- 13 files changed, 445 insertions(+), 651 deletions(-) create mode 100644 common/motion_utils/include/motion_utils/distance/distance.hpp create mode 100644 common/motion_utils/src/distance/distance.cpp create mode 100644 common/motion_utils/test/src/distance/test_distance.cpp diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 941855eb62528..31752cb851d79 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(Boost REQUIRED) ament_auto_add_library(motion_utils SHARED src/motion_utils.cpp + src/distance/distance.cpp src/marker/marker_helper.cpp src/resample/resample.cpp src/trajectory/interpolation.cpp diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/motion_utils/include/motion_utils/distance/distance.hpp new file mode 100644 index 0000000000000..2e25f5f0e53d7 --- /dev/null +++ b/common/motion_utils/include/motion_utils/distance/distance.hpp @@ -0,0 +1,34 @@ +// 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 MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#define MOTION_UTILS__DISTANCE__DISTANCE_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace motion_utils +{ +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec); + +} // namespace motion_utils + +#endif // MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp new file mode 100644 index 0000000000000..94d5cfeccecc3 --- /dev/null +++ b/common/motion_utils/src/distance/distance.cpp @@ -0,0 +1,267 @@ +// 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 "motion_utils/distance/distance.hpp" + +namespace motion_utils +{ +namespace +{ +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + + if (v_end < v_min || v_max < v_end) { + std::cerr << "[validCheckDecelPlan] valid check error! v_target = " << v_target + << ", v_end = " << v_end << std::endl; + return false; + } + if (a_end < a_min || a_max < a_end) { + std::cerr << "[validCheckDecelPlan] valid check error! a_target = " << a_target + << ", a_end = " << a_end << std::endl; + return false; + } + + return true; +} + +/** + * @brief update traveling distance, velocity and acceleration under constant jerk. + * @param (x) current traveling distance [m/s] + * @param (v) current velocity [m/s] + * @param (a) current acceleration [m/ss] + * @param (j) target jerk [m/sss] + * @param (t) time [s] + * @return updated traveling distance, velocity and acceleration + */ +std::tuple update( + const double x, const double v, const double a, const double j, const double t) +{ + const double a_new = a + j * t; + const double v_new = v + a * t + 0.5 * j * t * t; + const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; + + return {x_new, v_new, a_new}; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID + * ACCELERATION PROFILE). this type of profile has ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-------------------*------> t + * | * * + * | * * + * | a1 *************** + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----***************---------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (vt) target velocity [m/s] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min) +{ + constexpr double epsilon = 1e-3; + + // negative jerk time + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); + + // zero jerk time + const double t2 = epsilon < t_min ? t_min : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); + + // positive jerk time + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + + if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE + * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-----*--------------------> t + * | * * + * | * * + * | a1 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----*-----------------------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + // negative jerk time + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); + + // positive jerk time + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION + * PROFILE). This type of profile has only positive jerk time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * ----+----*-----------------------> t + * | * + * | * + * | * + * |* + * a0 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * ja ****** + * | * + * | * + * ----+----*-----------------------> t + * | + * + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + // positive jerk time + const double t_acc = (0.0 - a0) / ja; + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} +} // namespace + +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + constexpr double epsilon = 1e-3; + const double t_dec = + acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; + const double t_acc = (0.0 - acc_min) / jerk_acc; + const double t_min = (target_vel - current_vel - current_acc * t_dec - + 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_min > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + } else if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } + + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); +} +} // namespace motion_utils diff --git a/common/motion_utils/test/src/distance/test_distance.cpp b/common/motion_utils/test/src/distance/test_distance.cpp new file mode 100644 index 0000000000000..b5b9c5cfdc95f --- /dev/null +++ b/common/motion_utils/test/src/distance/test_distance.cpp @@ -0,0 +1,116 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "motion_utils/distance/distance.hpp" +namespace +{ +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +constexpr double epsilon = 1e-3; + +TEST(distance, calcDecelDistWithJerkAndAccConstraints) +{ + // invalid velocity + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 20.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + + EXPECT_FALSE(dist); + } + + // invalid deceleration + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = -2.5; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + + EXPECT_FALSE(dist); + } + + // normal stop + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + constexpr double expected_dist = 287.224; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // sudden stop + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -2.5; + constexpr double jerk_acc = 1.5; + constexpr double jerk_dec = -1.5; + + constexpr double expected_dist = 69.6947; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // normal deceleration + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 10.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + constexpr double expected_dist = 189.724; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // sudden deceleration + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 10.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -2.5; + constexpr double jerk_acc = 1.5; + constexpr double jerk_dec = -1.5; + + constexpr double expected_dist = 58.028; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } +} + +} // namespace diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index a02829aa6f298..f982890c8528c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -359,9 +359,9 @@ class AvoidanceModule : public SceneModuleInterface path_shifter.removeBehindShiftLineAndSetBaseOffset(nearest_idx); } - double getFeasibleDecelDistance(const double target_velocity) const; + boost::optional getFeasibleDecelDistance(const double target_velocity) const; - double getMildDecelDistance(const double target_velocity) const; + boost::optional getMildDecelDistance(const double target_velocity) const; double getRelativeLengthFromPath(const AvoidLine & avoid_line) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp index c69446c4d5ea8..8e4f6f6fd8e28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp @@ -114,10 +114,6 @@ lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); -double calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec); - void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 681cf7c1072df..ce779d97bebc4 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -43,6 +44,7 @@ namespace behavior_path_planner { +using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -3863,7 +3865,8 @@ void AvoidanceModule::updateAvoidanceDebugData( } } -double AvoidanceModule::getFeasibleDecelDistance(const double target_velocity) const +boost::optional AvoidanceModule::getFeasibleDecelDistance( + const double target_velocity) const { const auto & a_now = planner_data_->self_acceleration->accel.accel.linear.x; const auto & a_lim = parameters_->max_deceleration; @@ -3872,7 +3875,7 @@ double AvoidanceModule::getFeasibleDecelDistance(const double target_velocity) c getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); } -double AvoidanceModule::getMildDecelDistance(const double target_velocity) const +boost::optional AvoidanceModule::getMildDecelDistance(const double target_velocity) const { const auto & a_now = planner_data_->self_acceleration->accel.accel.linear.x; const auto & a_lim = parameters_->nominal_deceleration; @@ -3943,11 +3946,11 @@ void AvoidanceModule::insertWaitPoint( } const auto stop_distance = getMildDecelDistance(0.0); - - const auto insert_distance = std::max(start_longitudinal, stop_distance); - - insertDecelPoint( - getEgoPosition(), insert_distance, 0.0, shifted_path.path, debug_data_.stop_pose); + if (stop_distance) { + const auto insert_distance = std::max(start_longitudinal, *stop_distance); + insertDecelPoint( + getEgoPosition(), insert_distance, 0.0, shifted_path.path, debug_data_.stop_pose); + } } void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const @@ -3964,9 +3967,11 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const } const auto decel_distance = getMildDecelDistance(p->yield_velocity); - - insertDecelPoint( - getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, debug_data_.slow_pose); + if (decel_distance) { + insertDecelPoint( + getEgoPosition(), *decel_distance, p->yield_velocity, shifted_path.path, + debug_data_.slow_pose); + } } void AvoidanceModule::insertPrepareVelocity(const bool avoidable, ShiftedPath & shifted_path) const @@ -3992,8 +3997,10 @@ void AvoidanceModule::insertPrepareVelocity(const bool avoidable, ShiftedPath & } const auto decel_distance = getFeasibleDecelDistance(0.0); - - insertDecelPoint(getEgoPosition(), decel_distance, 0.0, shifted_path.path, debug_data_.slow_pose); + if (decel_distance) { + insertDecelPoint( + getEgoPosition(), *decel_distance, 0.0, shifted_path.path, debug_data_.slow_pose); + } } std::shared_ptr AvoidanceModule::get_debug_msg_array() const diff --git a/planning/behavior_path_planner/src/util/avoidance/util.cpp b/planning/behavior_path_planner/src/util/avoidance/util.cpp index 6681fd19e7d00..5e7de8f2e2e6a 100644 --- a/planning/behavior_path_planner/src/util/avoidance/util.cpp +++ b/planning/behavior_path_planner/src/util/avoidance/util.cpp @@ -62,174 +62,6 @@ geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygo return ret; } -/** - * @brief update traveling distance, velocity and acceleration under constant jerk. - * @param (x) current traveling distance [m/s] - * @param (v) current velocity [m/s] - * @param (a) current acceleration [m/ss] - * @param (j) target jerk [m/sss] - * @param (t) time [s] - * @return updated traveling distance, velocity and acceleration - */ -std::tuple update( - const double x, const double v, const double a, const double j, const double t) -{ - const double a_new = a + j * t; - const double v_new = v + a * t + 0.5 * j * t * t; - const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; - - return {x_new, v_new, a_new}; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID ACCELERATION - * PROFILE). this type of profile has ZERO JERK time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * a0 * - * |* - * ----+-*-------------------*------> t - * | * * - * | * * - * | a1 *************** - * | - * - * [JERK PROFILE] - * j ^ - * | - * | ja **** - * | * - * ----+----***************---------> t - * | * - * | * - * jd ****** - * | - * - * @param (v0) current velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - */ -double calcDecelDistPlanType1( - const double v0, const double a0, const double am, const double ja, const double jd, - const double t_min) -{ - constexpr double epsilon = 1e-3; - - // negative jerk time - const double j1 = am < a0 ? jd : ja; - const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; - const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); - - // zero jerk time - const double t2 = epsilon < t_min ? t_min : 0.0; - const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); - - // positive jerk time - const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; - const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); - - return x3; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE ACCELERATION - * PROFILE), This type of profile do NOT have ZERO JERK time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * a0 * - * |* - * ----+-*-----*--------------------> t - * | * * - * | * * - * | a1 * - * | - * - * [JERK PROFILE] - * j ^ - * | - * | ja **** - * | * - * ----+----*-----------------------> t - * | * - * | * - * jd ****** - * | - * - * @param (v0) current velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - */ -double calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd) -{ - constexpr double epsilon = 1e-3; - - const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); - const double a1 = -std::sqrt(a1_square); - - // negative jerk time - const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; - const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); - - // positive jerk time - const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; - const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); - - return x2; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION - * PROFILE). This type of profile has only positive jerk time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * ----+----*-----------------------> t - * | * - * | * - * | * - * |* - * a0 * - * | - * - * [JERK PROFILE] - * j ^ - * | - * ja ****** - * | * - * | * - * ----+----*-----------------------> t - * | - * - * @param (v0) current velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - */ -double calcDecelDistPlanType3(const double v0, const double a0, const double ja) -{ - constexpr double epsilon = 1e-3; - - // positive jerk time - const double t_acc = (0.0 - a0) / ja; - const double t1 = epsilon < t_acc ? t_acc : 0.0; - const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); - - return x1; -} - boost::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) @@ -952,32 +784,6 @@ lanelet::ConstLanelets getTargetLanelets( return target_lanelets; } -double calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec) -{ - constexpr double epsilon = 1e-3; - const double t_dec = - acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; - const double t_acc = (0.0 - acc_min) / jerk_acc; - const double t_min = (target_vel - current_vel - current_acc * t_dec - - 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / - acc_min; - - // check if it is possible to decelerate to the target velocity - // by simply bringing the current acceleration to zero. - const auto is_decel_needed = - 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - - if (t_min > epsilon) { - return calcDecelDistPlanType1(current_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); - } else if (is_decel_needed || current_acc > epsilon) { - return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); - } - - return calcDecelDistPlanType3(current_vel, current_acc, jerk_acc); -} - void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out) diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp index 915988aa4a84a..39f41e9da11ff 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp @@ -18,6 +18,7 @@ #include "behavior_velocity_planner/planner_data.hpp" #include "utilization/util.hpp" +#include #include #include @@ -177,56 +178,6 @@ struct DynamicObstacleData Polygons2d mandatory_detection_area; }; -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin); - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE1) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_min); - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE2) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd); - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE3) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja); - -boost::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec); - Polygon2d createBoostPolyFromMsg(const std::vector & input_poly); std::uint8_t getHighestProbLabel(const std::vector & classification); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index c24b2d1c6c2bd..ee8c2e5d0ab75 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -536,7 +536,7 @@ boost::optional RunOutModule::calcStopPoint( const float planning_dec = jerk_dec < planner_param_.common.normal_min_jerk ? planner_param_.common.limit_min_acc : planner_param_.common.normal_min_acc; - auto stop_dist = run_out_utils::calcDecelDistWithJerkAndAccConstraints( + auto stop_dist = motion_utils::calcDecelDistWithJerkAndAccConstraints( current_vel, target_vel, current_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { RCLCPP_WARN_STREAM(logger_, "failed to calculate stop distance."); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp index 2b7c03f49f18a..2093d0a01f453 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -18,172 +18,6 @@ namespace behavior_velocity_planner { namespace run_out_utils { -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin) -{ - const double v_min = v_target - std::abs(v_margin); - const double v_max = v_target + std::abs(v_margin); - const double a_min = a_target - std::abs(a_margin); - const double a_max = a_target + std::abs(a_margin); - - if (v_end < v_min || v_max < v_end) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("validCheckDecelPlan"), - "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); - return false; - } - if (a_end < a_min || a_max < a_end) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("validCheckDecelPlan"), - "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); - return false; - } - - return true; -} -/** - * @brief calculate distance until velocity is reached target velocity (TYPE1) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_min) -{ - constexpr double epsilon = 1e-3; - - const double j1 = am < a0 ? jd : ja; - const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; - const double a1 = a0 + j1 * t1; - const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; - - const double t2 = epsilon < t_min ? t_min : 0.0; - const double a2 = a1; - const double v2 = v1 + a1 * t2; - const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; - - const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; - const double a3 = a2 + ja * t3; - const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; - const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * t3 * t3 * t3; - - const double a_target = 0.0; - const double v_margin = 0.3; // [m/s] - const double a_margin = 0.1; // [m/s^2] - - if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x3; -} -/** - * @brief calculate distance until velocity is reached target velocity (TYPE2) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd) -{ - constexpr double epsilon = 1e-3; - - const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); - const double a1 = -std::sqrt(a1_square); - - const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; - const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; - - const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; - const double a2 = a1 + ja * t2; - const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; - const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x2; -} -/** - * @brief calculate distance until velocity is reached target velocity (TYPE3) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja) -{ - constexpr double epsilon = 1e-3; - - const double t_acc = (0.0 - a0) / ja; - - const double t1 = epsilon < t_acc ? t_acc : 0.0; - const double a1 = a0 + ja * t1; - const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x1; -} -boost::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec) -{ - constexpr double epsilon = 1e-3; - const double t_dec = - acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; - const double t_acc = (0.0 - acc_min) / jerk_acc; - const double t_min = (target_vel - current_vel - current_acc * t_dec - - 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / - acc_min; - - // check if it is possible to decelerate to the target velocity - // by simply bringing the current acceleration to zero. - const auto is_decel_needed = - 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - - if (t_min > epsilon) { - return calcDecelDistPlanType1( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); - } else if (is_decel_needed || current_acc > epsilon) { - return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); - } else { - return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); - } - - return {}; -} - Polygon2d createBoostPolyFromMsg(const std::vector & input_poly) { Polygon2d bg_poly; @@ -502,7 +336,7 @@ Polygons2d createDetectionAreaPolygon( const float jerk_acc = std::abs(jerk_dec); const float planning_dec = jerk_dec < pp.common.normal_min_jerk ? pp.common.limit_min_acc : pp.common.normal_min_acc; - auto stop_dist = run_out_utils::calcDecelDistWithJerkAndAccConstraints( + auto stop_dist = motion_utils::calcDecelDistWithJerkAndAccConstraints( initial_vel, target_vel, initial_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { @@ -546,7 +380,7 @@ Polygons2d createMandatoryDetectionAreaPolygon( const float jerk_acc = std::abs(jerk_dec); const float planning_dec = jerk_dec < pp.common.normal_min_jerk ? pp.common.limit_min_acc : pp.common.normal_min_acc; - auto stop_dist = run_out_utils::calcDecelDistWithJerkAndAccConstraints( + auto stop_dist = motion_utils::calcDecelDistWithJerkAndAccConstraints( initial_vel, target_vel, initial_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index 5683f1de51d9c..a07fcbc537855 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -58,56 +58,6 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin); - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE1) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_min); - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE2) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd); - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE3) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja); - -boost::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec); - boost::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, const double current_vel, const double current_acc); diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index a473e6ea3ac50..5fba295e3e1d2 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -14,6 +14,7 @@ #include "obstacle_stop_planner/planner_utils.hpp" +#include #include #include @@ -26,181 +27,12 @@ namespace motion_planning { +using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::getRPY; -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin) -{ - const double v_min = v_target - std::abs(v_margin); - const double v_max = v_target + std::abs(v_margin); - const double a_min = a_target - std::abs(a_margin); - const double a_max = a_target + std::abs(a_margin); - - if (v_end < v_min || v_max < v_end) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("validCheckDecelPlan"), - "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); - return false; - } - if (a_end < a_min || a_max < a_end) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("validCheckDecelPlan"), - "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); - return false; - } - - return true; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE1) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_min) -{ - constexpr double epsilon = 1e-3; - - const double j1 = am < a0 ? jd : ja; - const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; - const double a1 = a0 + j1 * t1; - const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; - - const double t2 = epsilon < t_min ? t_min : 0.0; - const double a2 = a1; - const double v2 = v1 + a1 * t2; - const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; - - const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; - const double a3 = a2 + ja * t3; - const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; - const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * t3 * t3 * t3; - - const double a_target = 0.0; - const double v_margin = 0.3; // [m/s] - const double a_margin = 0.1; // [m/s^2] - - if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x3; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE2) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd) -{ - constexpr double epsilon = 1e-3; - - const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); - const double a1 = -std::sqrt(a1_square); - - const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; - const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; - - const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; - const double a2 = a1 + ja * t2; - const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; - const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x2; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE3) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja) -{ - constexpr double epsilon = 1e-3; - - const double t_acc = (0.0 - a0) / ja; - - const double t1 = epsilon < t_acc ? t_acc : 0.0; - const double a1 = a0 + ja * t1; - const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x1; -} - -boost::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec) -{ - constexpr double epsilon = 1e-3; - const double t_dec = - acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; - const double t_acc = (0.0 - acc_min) / jerk_acc; - const double t_min = (target_vel - current_vel - current_acc * t_dec - - 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / - acc_min; - - // check if it is possible to decelerate to the target velocity - // by simply bringing the current acceleration to zero. - const auto is_decel_needed = - 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - - if (t_min > epsilon) { - return calcDecelDistPlanType1( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); - } else if (is_decel_needed || current_acc > epsilon) { - return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); - } else { - return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); - } - - return {}; -} - boost::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, const double current_vel, const double current_acc) From 7ecd2cf83b30fa3d4f797d77cdc207abac02bca6 Mon Sep 17 00:00:00 2001 From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com> Date: Fri, 31 Mar 2023 12:51:03 +0900 Subject: [PATCH 13/19] refactor(planning_debug_tools): delete default values (#3126) delete param Signed-off-by: yamazakiTasuku Co-authored-by: yamazakiTasuku --- planning/planning_debug_tools/src/trajectory_analyzer.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/planning/planning_debug_tools/src/trajectory_analyzer.cpp b/planning/planning_debug_tools/src/trajectory_analyzer.cpp index 5545eb1d74e6e..2dfcf9401f79b 100644 --- a/planning/planning_debug_tools/src/trajectory_analyzer.cpp +++ b/planning/planning_debug_tools/src/trajectory_analyzer.cpp @@ -20,10 +20,9 @@ TrajectoryAnalyzerNode::TrajectoryAnalyzerNode(const rclcpp::NodeOptions & optio : Node("trajectory_analyzer", options) { using TopicNames = std::vector; - const auto path_topics = declare_parameter("path_topics", TopicNames{}); - const auto path_with_lane_id_topics = - declare_parameter("path_with_lane_id_topics", TopicNames{}); - const auto trajectory_topics = declare_parameter("trajectory_topics", TopicNames{}); + const auto path_topics = declare_parameter("path_topics"); + const auto path_with_lane_id_topics = declare_parameter("path_with_lane_id_topics"); + const auto trajectory_topics = declare_parameter("trajectory_topics"); for (const auto & s : path_topics) { path_analyzers_.push_back(std::make_shared>(this, s)); From 1691baf8d4573ddeb98f7f053b0f779abb4a3a55 Mon Sep 17 00:00:00 2001 From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com> Date: Fri, 31 Mar 2023 13:00:56 +0900 Subject: [PATCH 14/19] refactor(freespace_planner): delete default values (#3128) delete param Signed-off-by: yamazakiTasuku Co-authored-by: yamazakiTasuku --- .../freespace_planner_node.cpp | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 1dc81f6fee1a0..6919b4eac142f 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -226,16 +226,16 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti // NodeParam { auto & p = node_param_; - p.planning_algorithm = declare_parameter("planning_algorithm", "astar"); - p.waypoints_velocity = declare_parameter("waypoints_velocity", 5.0); - p.update_rate = declare_parameter("update_rate", 1.0); - p.th_arrived_distance_m = declare_parameter("th_arrived_distance_m", 1.0); - p.th_stopped_time_sec = declare_parameter("th_stopped_time_sec", 1.0); - p.th_stopped_velocity_mps = declare_parameter("th_stopped_velocity_mps", 0.01); - p.th_course_out_distance_m = declare_parameter("th_course_out_distance_m", 3.0); - p.vehicle_shape_margin_m = declare_parameter("vehicle_shape_margin_m", 1.0); - p.replan_when_obstacle_found = declare_parameter("replan_when_obstacle_found", true); - p.replan_when_course_out = declare_parameter("replan_when_course_out", true); + p.planning_algorithm = declare_parameter("planning_algorithm"); + p.waypoints_velocity = declare_parameter("waypoints_velocity"); + p.update_rate = declare_parameter("update_rate"); + p.th_arrived_distance_m = declare_parameter("th_arrived_distance_m"); + p.th_stopped_time_sec = declare_parameter("th_stopped_time_sec"); + p.th_stopped_velocity_mps = declare_parameter("th_stopped_velocity_mps"); + p.th_course_out_distance_m = declare_parameter("th_course_out_distance_m"); + p.vehicle_shape_margin_m = declare_parameter("vehicle_shape_margin_m"); + p.replan_when_obstacle_found = declare_parameter("replan_when_obstacle_found"); + p.replan_when_course_out = declare_parameter("replan_when_course_out"); } // set vehicle_info @@ -291,22 +291,22 @@ PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() PlannerCommonParam p; // search configs - p.time_limit = declare_parameter("time_limit", 5000.0); - p.minimum_turning_radius = declare_parameter("minimum_turning_radius", 0.5); - p.maximum_turning_radius = declare_parameter("maximum_turning_radius", 6.0); - p.turning_radius_size = declare_parameter("turning_radius_size", 11); + p.time_limit = declare_parameter("time_limit"); + p.minimum_turning_radius = declare_parameter("minimum_turning_radius"); + p.maximum_turning_radius = declare_parameter("maximum_turning_radius"); + p.turning_radius_size = declare_parameter("turning_radius_size"); p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); p.turning_radius_size = std::max(p.turning_radius_size, 1); - p.theta_size = declare_parameter("theta_size", 48); - p.angle_goal_range = declare_parameter("angle_goal_range", 6.0); - p.curve_weight = declare_parameter("curve_weight", 1.2); - p.reverse_weight = declare_parameter("reverse_weight", 2.00); - p.lateral_goal_range = declare_parameter("lateral_goal_range", 0.5); - p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range", 2.0); + p.theta_size = declare_parameter("theta_size"); + p.angle_goal_range = declare_parameter("angle_goal_range"); + p.curve_weight = declare_parameter("curve_weight"); + p.reverse_weight = declare_parameter("reverse_weight"); + p.lateral_goal_range = declare_parameter("lateral_goal_range"); + p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range"); // costmap configs - p.obstacle_threshold = declare_parameter("obstacle_threshold", 100); + p.obstacle_threshold = declare_parameter("obstacle_threshold"); return p; } From 8e38d0d388e1ba355eaadbd82ed377a03b89b686 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 31 Mar 2023 15:52:30 +0900 Subject: [PATCH 15/19] feat(motion_utils): add insertDecelPoint (#3226) * feat(motion_utils): add insertDecelPoint Signed-off-by: kosuke55 * not use lamda function Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../motion_utils/trajectory/trajectory.hpp | 36 ++++++++ .../test/src/trajectory/test_trajectory.cpp | 90 +++++++++++++++++++ 2 files changed, 126 insertions(+) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index dbdfc251164b4..327aede40b2a3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1245,6 +1245,42 @@ inline boost::optional insertStopPoint( return stop_idx; } +/** + * @brief Insert deceleration point from the source pose + * @param src_point source point + * @param distance_to_decel_point distance to deceleration point from the src point + * @param velocity velocity of stop point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + */ +template +boost::optional insertDecelPoint( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, T & points_with_twist) +{ + const auto decel_point = + calcLongitudinalOffsetPoint(points_with_twist, src_point, distance_to_decel_point); + + if (!decel_point) { + return {}; + } + + const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.get()); + const auto insert_idx = insertTargetPoint(seg_idx, decel_point.get(), points_with_twist); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.get(); i < points_with_twist.size(); ++i) { + const auto & original_velocity = + tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(i)); + tier4_autoware_utils::setLongitudinalVelocity( + std::min(original_velocity, velocity), points_with_twist.at(i)); + } + + return insert_idx; +} + /** * @brief Insert orientation to each point in points container (trajectory, path, ...) * @param points points of trajectory, path, ... (input / output) diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 66ce3ea3e697c..5ffb8a612a308 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -3742,6 +3742,96 @@ TEST(trajectory, insertStopPoint_from_a_pose) } } +TEST(trajectory, insertDecelPoint_from_a_point) +{ + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertDecelPoint; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::getLongitudinalVelocity; + + const auto traj = generateTestTrajectory(10, 1.0, 10.0); + const auto total_length = calcArcLength(traj.points); + const double decel_velocity = 5.0; + + // Insert (From Zero Point) + { + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + const geometry_msgs::msg::Point src_point = createPoint(0.0, 0.0, 0.0); + const auto p_target = createPoint(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.get()) { + EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); + } else { + EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); + } + } + } + } + + // Insert (From Non-Zero Point) + { + const double src_point_x = 5.0; + const double src_point_y = 3.0; + for (double x_start = 0.5; x_start < total_length - src_point_x; x_start += 1.0) { + auto traj_out = traj; + const geometry_msgs::msg::Point src_point = createPoint(src_point_x, src_point_y, 0.0); + const auto p_target = createPoint(src_point_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.get()) { + EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); + } else { + EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); + } + } + } + } + + // No Insert + { + const double src_point_x = 2.0; + const double src_point_y = 3.0; + for (double x_start = 1e-3; x_start < total_length - src_point_x; x_start += 1.0) { + auto traj_out = traj; + const geometry_msgs::msg::Point src_point = createPoint(src_point_x, src_point_y, 0.0); + const auto p_target = createPoint(src_point_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.get()) { + EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); + } else { + EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); + } + } + } + } +} + TEST(trajectory, findFirstNearestIndexWithSoftConstraints) { using motion_utils::findFirstNearestIndexWithSoftConstraints; From b043f48539cf351efc150298a289cb5a23fc1aff Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Fri, 31 Mar 2023 17:51:54 +0900 Subject: [PATCH 16/19] docs(pose_initializer): update initialization flow diagram (#3228) * update localization.drawio.svg Signed-off-by: Kento Yabuuchi * fix Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../document/images/localization.drawio.svg | 402 +++++++++++------- 1 file changed, 250 insertions(+), 152 deletions(-) diff --git a/system/default_ad_api/document/images/localization.drawio.svg b/system/default_ad_api/document/images/localization.drawio.svg index 3c1347e8b4a3c..3695919cc7059 100644 --- a/system/default_ad_api/document/images/localization.drawio.svg +++ b/system/default_ad_api/document/images/localization.drawio.svg @@ -3,26 +3,26 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="922px" + width="1042px" height="681px" - viewBox="-0.5 -0.5 922 681" - content="<mxfile><diagram id="6E-LtnesXNhnNIPTZu3a" name="Page-1">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</diagram></mxfile>" + viewBox="-0.5 -0.5 1042 681" + content="<mxfile><diagram id="6E-LtnesXNhnNIPTZu3a" name="Page-1">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</diagram></mxfile>" > - - - - - - - + + + + + + +
@@ -31,18 +31,18 @@
- Applications (FMS, etc.) + Applications (FMS, etc.) - - - + + +
@@ -54,15 +54,15 @@ /api/localization/initialize - - - + + +
@@ -73,20 +73,20 @@
- AD API... + AD API... - - - - - + + + + +
@@ -95,16 +95,16 @@
- /localization/initialize + /localization/initialize - +
@@ -113,18 +113,18 @@
- component_state_monitor + component_state_monitor - - - + + +
@@ -133,21 +133,21 @@
- initialpose3d + initialpose3d - - - - - - + + + + + +
@@ -156,14 +156,17 @@
- simple_planning_simulator + simple_planning_simulator - + -
+
NDT @@ -171,17 +174,20 @@
- NDT + NDT - - - - + + + + -
+
ndt_align_srv @@ -189,14 +195,14 @@
- ndt_align_srv + ndt_align_srv - + -
+
common @@ -204,14 +210,14 @@
- common + common - + -
+
psim @@ -219,14 +225,14 @@
- psim + psim - + -
+
lsim/real @@ -234,14 +240,14 @@
- lsim/real + lsim/real - + -
+
node @@ -249,14 +255,14 @@
- node + node - + -
+
service @@ -264,14 +270,14 @@
- service + service - + -
+
topic @@ -279,23 +285,21 @@
- topic + topic - - - - - - - - + + + + + +
@@ -308,18 +312,18 @@
- initial_pose_adaptor... + initial_pose_adaptor... - - - + + +
@@ -328,18 +332,20 @@
- EKF + EKF - - - + + + + +
@@ -348,206 +354,298 @@
- gnss pose + gnss pose - + + +
-
- Module when using GNSS +
+ automatic_pose_initializer
- (Fix z position to fit map) + (Call the API when state is uninitialized)
- Module when using GNSS... + automatic_pose_initializer... - - - + + +
- automatic_pose_initializer -
- (Call the API when state is uninitialized) + initialpose (from rviz)
- automatic_pose_initializer... + initialpose (from rviz) - - - + + + +
- initialpose (from rviz) + /map/get_partial_pointcloud_map
- initialpose (from rviz) + /map/get_partial_pointcloud_map - - - - - - - + -
+
- fit_map_height + pointcloud_map_loader
- fit_map_height + pointcloud_map_loader - + + + -
+
- map_height_fitter + /localization/initialization_state
- map_height_fitter + /localization/initialization_state - + + +
-
- Module when using NDT +
+ sensing twist
- Module when using NDT + sensing twist - - - +
- /localization/initialization_state + pose_initializer +
+
+
+ + pose_initializer + + + + + + +
+
+
+ module
- /localization/initialization_state + module - - - + + +
- sensing twist + EKF trigger
- sensing twist + EKF trigger - +
- pose_initializer + stop check +
+
+
+ + stop check + + + + + + + + +
+
+
+ trigger_node +
+
+
+ + trigger_node + + + + + + +
+
+
+ GNSS +
+ (Fix z position to fit map)
- pose_initializer + GNSS... - +
-
- Module to check vehicle stops +
+ NDT align +
+
+
+ + NDT align + + + + + + + + +
+
+
+ NDT trigger +
+
+
+ + NDT trigger + + + + + + + + +
+
+
+ trigger_node
- Module to check vehicle stops + trigger_node - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display From 029d038ec85c6422c7d22b1d4736f1cef77c0042 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 31 Mar 2023 17:53:48 +0900 Subject: [PATCH 17/19] feat(map_loader): add grid coordinates for partial/differential map load (#3205) * feat(map_loader): add grid coordinates for partial/differential map load Signed-off-by: kminoda * style(pre-commit): autofix * update readme Signed-off-by: kminoda * remove unnecessary line * update arguments in readme Signed-off-by: kminoda * slightly updated directory structure in readme Signed-off-by: kminoda * update readme Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- launch/tier4_map_launch/launch/map.launch.py | 6 ++ map/map_loader/CMakeLists.txt | 4 ++ map/map_loader/README.md | 58 ++++++++++++++-- .../launch/pointcloud_map_loader.launch.xml | 2 + map/map_loader/package.xml | 1 + .../differential_map_loader_module.cpp | 4 ++ .../partial_map_loader_module.cpp | 6 +- .../pointcloud_map_loader_node.cpp | 67 ++++++++++++------- .../pointcloud_map_loader_node.hpp | 4 +- .../src/pointcloud_map_loader/utils.cpp | 47 +++++++++++++ .../src/pointcloud_map_loader/utils.hpp | 15 +++++ .../test/test_load_pcd_metadata.cpp | 55 +++++++++++++++ .../test_pointcloud_map_loader_module.cpp | 9 +-- .../test/test_replace_with_absolute_path.cpp | 65 ++++++++++++++++++ 14 files changed, 301 insertions(+), 42 deletions(-) create mode 100644 map/map_loader/test/test_load_pcd_metadata.cpp create mode 100644 map/map_loader/test/test_replace_with_absolute_path.cpp diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 867eb99b8f8b3..2624b4244ec8e 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -89,6 +89,7 @@ def launch_setup(context, *args, **kwargs): ], parameters=[ {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, + {"pcd_metadata_path": [LaunchConfiguration("pointcloud_map_metadata_path")]}, pointcloud_map_loader_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -151,6 +152,11 @@ def add_launch_arg(name: str, default_value=None, description=None): [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"], "path to pointcloud map file", ), + add_launch_arg( + "pointcloud_map_metadata_path", + [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], + "path to pointcloud map metadata file", + ), add_launch_arg( "lanelet2_map_loader_param_path", [ diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index b8abb4da84e4a..9cedb28edf539 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) +target_link_libraries(pointcloud_map_loader_node yaml-cpp) target_include_directories(pointcloud_map_loader_node SYSTEM PUBLIC @@ -59,12 +60,15 @@ if(BUILD_TESTING) ament_add_gmock(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(${test_name} ${${PROJECT_NAME}_LIBRARIES}) + target_link_libraries(${test_name} yaml-cpp) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_sphere_box_overlap.cpp) + add_testcase(test/test_replace_with_absolute_path.cpp) + add_testcase(test/test_load_pcd_metadata.cpp) add_testcase(test/test_pointcloud_map_loader_module.cpp) add_testcase(test/test_partial_map_loader_module.cpp) add_testcase(test/test_differential_map_loader_module.cpp) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index c8f5bed19bb3d..80d42e727dff6 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -38,13 +38,15 @@ Please see [the description of `GetDifferentialPointCloudMap.srv`](https://githu ### Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :---- | :-------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | -| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | +| Name | Type | Description | Default value | +| :---------------------------- | :---------- | :-------------------------------------------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | false | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | +| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | +| pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | +| pcd_metadata_path | std::string | Path to pointcloud metadata file | | ### Interfaces @@ -52,6 +54,48 @@ Please see [the description of `GetDifferentialPointCloudMap.srv`](https://githu - `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map - `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map +- pointcloud map file(s) (.pcd) +- metadata of pointcloud map(s) (.yaml) + +### Metadata + +You must provide metadata in YAML format as well as pointcloud map files. Pointcloud map should be divided into one or more files with x-y grid. + +Metadata should look like this: + +```yaml +x_resolution: 100.0 +y_resolution: 150.0 +A.pcd: [1200, 2500] # -> 1200 < x < 1300, 2500 < y < 2650 +B.pcd: [1300, 2500] # -> 1300 < x < 1400, 2500 < y < 2650 +C.pcd: [1200, 2650] # -> 1200 < x < 1300, 2650 < y < 2800 +D.pcd: [1400, 2650] # -> 1400 < x < 1500, 2650 < y < 2800 +``` + +You may use [pointcloud_divider](https://github.com/MapIV/pointcloud_divider) from MAP IV for dividing pointcloud map as well as generating the compatible metadata.yaml. + +### How to store map-related files + +If you only have one pointcloud map, Autoware will assume the following directory structure by default. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +``` + +If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map +├── pcd_00.pcd +├── pcd_01.pcd +├── pcd_02.pcd +├── ... +└── pointcloud_map_metadata.yaml +``` --- diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 3cfc457c15d22..df32fab0cda1c 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -1,11 +1,13 @@ + + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 7a22a369744ab..4fa5cc4156794 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -29,6 +29,7 @@ tf2_ros tier4_autoware_utils visualization_msgs + yaml-cpp ament_cmake_gmock ament_lint_auto diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 8322ac5f60b99..1e81218c42410 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -48,6 +48,10 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id); + pointcloud_map_cell_with_id.min_x = metadata.min.x; + pointcloud_map_cell_with_id.min_y = metadata.min.y; + pointcloud_map_cell_with_id.max_x = metadata.max.x; + pointcloud_map_cell_with_id.max_y = metadata.max.y; response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 1454b01147ac2..234f78c2a071e 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -29,7 +29,6 @@ void PartialMapLoaderModule::partialAreaLoad( GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids - for (const auto & ele : all_pcd_file_metadata_dict_) { std::string path = ele.first; PCDFileMetadata metadata = ele.second; @@ -42,6 +41,11 @@ void PartialMapLoaderModule::partialAreaLoad( autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id); + pointcloud_map_cell_with_id.min_x = metadata.min.x; + pointcloud_map_cell_with_id.min_y = metadata.min.y; + pointcloud_map_cell_with_id.max_x = metadata.max.x; + pointcloud_map_cell_with_id.max_y = metadata.max.y; + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index ecbe481345381..7feea26bcb073 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -49,6 +49,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt { const auto pcd_paths = getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + std::string pcd_metadata_path = declare_parameter("pcd_metadata_path"); bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); @@ -66,18 +67,52 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load | enable_differential_load) { - pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); - } + if (enable_partial_load || enable_differential_load) { + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } + + if (enable_differential_load) { + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict); + } } +} + +std::map PointCloudMapLoaderNode::getPCDMetadata( + const std::string & pcd_metadata_path, const std::vector & pcd_paths) const +{ + std::map pcd_metadata_dict; + if (pcd_paths.size() != 1) { + if (!fs::exists(pcd_metadata_path)) { + throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict_); + pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); + pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict_, pcd_paths); + RCLCPP_INFO_STREAM(get_logger(), "Loaded PCD metadata: " << pcd_metadata_path); + } else { + // An exception when using a single file PCD map so that the users do not have to provide + // a metadata file. + // Note that this should ideally be avoided and thus eventually be removed by someone, until + // Autoware users get used to handling the PCD file(s) with metadata. + RCLCPP_INFO_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); + pcl::PointCloud single_pcd; + if (pcl::io::loadPCDFile(pcd_paths[0], single_pcd) == -1) { + throw std::runtime_error("PCD load failed: " + pcd_paths[0]); + } + PCDFileMetadata metadata = {}; + pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); + pcd_metadata_dict[pcd_paths[0]] = metadata; } + return pcd_metadata_dict; } std::vector PointCloudMapLoaderNode::getPcdPaths( @@ -105,21 +140,5 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } -std::map PointCloudMapLoaderNode::generatePCDMetadata( - const std::vector & pcd_paths) const -{ - pcl::PointCloud partial_pcd; - std::map all_pcd_file_metadata_dict; - for (const auto & path : pcd_paths) { - if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { - RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path); - } - PCDFileMetadata metadata = {}; - pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max); - all_pcd_file_metadata_dict[path] = metadata; - } - return all_pcd_file_metadata_dict; -} - #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index d321902131a81..27c90080b7f9a 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -48,8 +48,8 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; - std::map generatePCDMetadata( - const std::vector & pcd_paths) const; + std::map getPCDMetadata( + const std::string & pcd_metadata_path, const std::vector & pcd_paths) const; }; #endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 8af8c0dfd8ec0..62a3f064d18e0 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -16,6 +16,53 @@ #include +#include +#include +#include + +std::map loadPCDMetadata(const std::string & pcd_metadata_path) +{ + YAML::Node config = YAML::LoadFile(pcd_metadata_path); + + std::map metadata; + + for (const auto & node : config) { + if ( + node.first.as() == "x_resolution" || + node.first.as() == "y_resolution") { + continue; + } + + std::string key = node.first.as(); + std::vector values = node.second.as>(); + + PCDFileMetadata fileMetadata; + fileMetadata.min.x = values[0]; + fileMetadata.min.y = values[1]; + fileMetadata.max.x = values[0] + config["x_resolution"].as(); + fileMetadata.max.y = values[1] + config["y_resolution"].as(); + + metadata[key] = fileMetadata; + } + + return metadata; +} + +std::map replaceWithAbsolutePath( + const std::map & pcd_metadata_path, + const std::vector & pcd_paths) +{ + std::map absolute_path_map; + for (const auto & path : pcd_paths) { + std::string filename = path.substr(path.find_last_of("/\\") + 1); + auto it = pcd_metadata_path.find(filename); + if (it != pcd_metadata_path.end()) { + absolute_path_map[path] = it->second; + } + } + return absolute_path_map; +} + bool sphereAndBoxOverlapExists( const geometry_msgs::msg::Point center, const double radius, const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point) diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 0a0d2912bb8c5..bbf55b72a2348 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -19,13 +19,28 @@ #include #include +#include + +#include +#include +#include struct PCDFileMetadata { pcl::PointXYZ min; pcl::PointXYZ max; + bool operator==(const PCDFileMetadata & other) const + { + return min.x == other.min.x && min.y == other.min.y && min.z == other.min.z && + max.x == other.max.x && max.y == other.max.y && max.z == other.max.z; + } }; +std::map loadPCDMetadata(const std::string & pcd_metadata_path); +std::map replaceWithAbsolutePath( + const std::map & pcd_metadata_path, + const std::vector & pcd_paths); + bool sphereAndBoxOverlapExists( const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, const pcl::PointXYZ position_max); diff --git a/map/map_loader/test/test_load_pcd_metadata.cpp b/map/map_loader/test/test_load_pcd_metadata.cpp new file mode 100644 index 0000000000000..a832489b6db99 --- /dev/null +++ b/map/map_loader/test/test_load_pcd_metadata.cpp @@ -0,0 +1,55 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/utils.hpp" + +#include + +#include +#include + +using ::testing::ContainerEq; + +std::string createYAMLFile() +{ + std::filesystem::path tmp_path = std::filesystem::temp_directory_path() / "temp_metadata.yaml"; + + std::ofstream ofs(tmp_path); + ofs << "file1.pcd: [1, 2]\n"; + ofs << "file2.pcd: [3, 4]\n"; + ofs << "x_resolution: 5\n"; + ofs << "y_resolution: 6\n"; + ofs.close(); + + return tmp_path.string(); +} + +TEST(LoadPCDMetadataTest, BasicFunctionality) +{ + std::string yaml_file_path = createYAMLFile(); + + std::map expected = { + {"file1.pcd", {{1, 2, 0}, {6, 8, 0}}}, + {"file2.pcd", {{3, 4, 0}, {8, 10, 0}}}, + }; + + auto result = loadPCDMetadata(yaml_file_path); + ASSERT_THAT(result, ContainerEq(expected)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index e28f5fce66ede..5c6912a9192bd 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -24,7 +24,6 @@ #include #include -#include #include using std::chrono_literals::operator""ms; @@ -57,13 +56,7 @@ class TestPointcloudMapLoaderModule : public ::testing::Test pcl::io::savePCDFileASCII(temp_pcd_path, cloud); } - void TearDown() override - { - // Delete the temporary PCD file - std::filesystem::remove(temp_pcd_path); - - rclcpp::shutdown(); - } + void TearDown() override { rclcpp::shutdown(); } }; TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp new file mode 100644 index 0000000000000..10680e05103ce --- /dev/null +++ b/map/map_loader/test/test_replace_with_absolute_path.cpp @@ -0,0 +1,65 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/utils.hpp" + +#include +#include + +using ::testing::ContainerEq; + +TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) +{ + std::map pcd_metadata_path = { + {"file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, + {"file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, + }; + + std::vector pcd_paths = { + "/home/user/pcd/file1.pcd", + "/home/user/pcd/file2.pcd", + }; + + std::map expected = { + {"/home/user/pcd/file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, + {"/home/user/pcd/file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, + }; + + auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); + ASSERT_THAT(result, ContainerEq(expected)); +} + +TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) +{ + std::map pcd_metadata_path = { + {"file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, + {"file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, + }; + + std::vector pcd_paths = { + "/home/user/pcd/non_matching_file1.pcd", + "/home/user/pcd/non_matching_file2.pcd", + }; + + std::map expected = {}; + + auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); + ASSERT_THAT(result, ContainerEq(expected)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} From a3a5b19d9ac49f611b3552a28a807a63a0e11beb Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 31 Mar 2023 18:14:26 +0900 Subject: [PATCH 18/19] chore(motion_utils): remove validCheckDecelPlan error message (#3232) Signed-off-by: kosuke55 --- common/motion_utils/src/distance/distance.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index 94d5cfeccecc3..6349d724c635c 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/motion_utils/src/distance/distance.cpp @@ -28,13 +28,9 @@ bool validCheckDecelPlan( const double a_max = a_target + std::abs(a_margin); if (v_end < v_min || v_max < v_end) { - std::cerr << "[validCheckDecelPlan] valid check error! v_target = " << v_target - << ", v_end = " << v_end << std::endl; return false; } if (a_end < a_min || a_max < a_end) { - std::cerr << "[validCheckDecelPlan] valid check error! a_target = " << a_target - << ", a_end = " << a_end << std::endl; return false; } From 70cf872517cad0e92cdbd92510c8ac28333efeb6 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 31 Mar 2023 19:11:16 +0900 Subject: [PATCH 19/19] fix(control_launch): add necessary parameter (#3235) Signed-off-by: yutaka --- launch/tier4_control_launch/launch/control.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 63f3523503219..c658b29ccd30b 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -82,6 +82,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), + "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"), }, nearest_search_param, trajectory_follower_node_param,